Skip to content

Commit c63dfe5

Browse files
jschalljschall
committed
add leds to bootloader
1 parent 0f75dfd commit c63dfe5

File tree

4 files changed

+372
-2
lines changed

4 files changed

+372
-2
lines changed

‎bootloader/Makefile

Lines changed: 6 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -21,13 +21,17 @@ can \
2121
can_autobaud \
2222
uavcan \
2323
uavcan_nodestatus_publisher \
24-
uavcan_allocatee
24+
uavcan_allocatee \
25+
spi_device \
26+
driver_profiLED \
27+
uavcan_timesync
2528

2629
MESSAGES_ENABLED = \
2730
uavcan.protocol.GetNodeInfo \
2831
uavcan.protocol.file.BeginFirmwareUpdate \
2932
uavcan.protocol.file.Read \
30-
uavcan.protocol.RestartNode
33+
uavcan.protocol.RestartNode \
34+
uavcan.equipment.indication.LightsCommand
3135

3236
LOAD_REGION = bl
3337

‎bootloader/include/framework_conf.h

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@
1515
#define BOOTLOADER_APP_THREAD lpwork_thread
1616
#define LOGGER_WORKER_THREAD lpwork_thread
1717
#define USB_SLCAN_WORKER_THREAD can_thread
18+
#define UAVCAN_TIMESYNC_WORKER_THREAD lpwork_thread
1819

1920
#define CAN_TRX_WORKER_THREAD can_thread
2021
#define CAN_EXPIRE_WORKER_THREAD can_thread

‎bootloader/src/led.c

Lines changed: 364 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,364 @@
1+
#include <modules/driver_profiLED/driver_profiLED.h>
2+
#include <modules/worker_thread/worker_thread.h>
3+
#include <common/ctor.h>
4+
#include <common/helpers.h>
5+
#include <modules/uavcan/uavcan.h>
6+
#include <uavcan.equipment.indication.LightsCommand.h>
7+
#include <hal.h>
8+
#include <math.h>
9+
#include <modules/timing/timing.h>
10+
#include <modules/uavcan_timesync/uavcan_timesync.h>
11+
#include <modules/param/param.h>
12+
13+
static struct spi_device_s led_spi;
14+
static struct worker_thread_timer_task_s timer_task;
15+
static void timer_task_func(struct worker_thread_timer_task_s* task);
16+
struct worker_thread_listener_task_s led_command_task;
17+
static void led_command_handler(size_t msg_size, const void* buf, void* ctx);
18+
19+
static uint8_t board_led_order[] = BOARD_LED_ORDER;
20+
21+
static struct profiLED_color_s led_off(uint32_t led_idx, void* ctx);
22+
static struct profiLED_color_s led_uavcan(uint32_t led_idx, void* ctx);
23+
static struct profiLED_color_s led_redgreen(uint32_t led_idx, void* ctx);
24+
static struct profiLED_color_s led_red(uint32_t led_idx, void* ctx);
25+
static struct profiLED_color_s led_green(uint32_t led_idx, void* ctx);
26+
static struct profiLED_color_s led_rainbow(uint32_t led_idx, void* ctx);
27+
static struct profiLED_color_s led_police(uint32_t led_idx, void* ctx);
28+
static struct profiLED_color_s led_police_left(uint32_t led_idx, void* ctx);
29+
static struct profiLED_color_s led_police_right(uint32_t led_idx, void* ctx);
30+
static struct profiLED_color_s led_sequence(uint32_t led_idx, void* ctx);
31+
static struct profiLED_color_s led_heading(uint32_t led_idx, void* ctx);
32+
33+
static profiLED_color_func led_mode_funcs[] = { led_off, led_uavcan, led_redgreen, led_red, led_green, led_rainbow, led_police, led_police_left, led_police_right, led_sequence, led_heading};
34+
#define NUM_LED_MODES (sizeof(led_mode_funcs)/sizeof(led_mode_funcs[0]))
35+
36+
static const float led_angle_param = 0;
37+
static const uint8_t led_power_mode_param = 1;
38+
static const uint8_t led_mode_param = 5;
39+
static const bool led_strobe = false;
40+
// PARAM_DEFINE_FLOAT32_PARAM_STATIC(led_angle_param, "LED_ANGLE_OFS", 0, -2*M_PI_F, 2*M_PI_F)
41+
// PARAM_DEFINE_UINT8_PARAM_STATIC(led_power_mode_param, "LED_POWER_MODE", 1, 0, 2)
42+
// PARAM_DEFINE_UINT8_PARAM_STATIC(led_mode_param, "LED_MODE", 5, 0, NUM_LED_MODES-1)
43+
// PARAM_DEFINE_BOOL_PARAM_STATIC(led_strobe, "LED_STROBE", false)
44+
45+
WORKER_THREAD_DECLARE_EXTERN(led_thread)
46+
47+
static struct profiLED_color_s uavcan_commanded_color;
48+
49+
RUN_ON(INIT_END) {
50+
if (profiLED_spi_dev_init(&led_spi, 5, 0, false, 100000)) {
51+
worker_thread_add_timer_task(&led_thread, &timer_task, timer_task_func, NULL, TIME_IMMEDIATE, false);
52+
}
53+
struct pubsub_topic_s* led_command_topic = uavcan_get_message_topic(0, &uavcan_equipment_indication_LightsCommand_descriptor);
54+
worker_thread_add_listener_task(&led_thread, &led_command_task, led_command_topic, led_command_handler, NULL);
55+
}
56+
57+
static struct profiLED_color_s led_off(uint32_t led_idx, void* ctx) {
58+
led_idx = board_led_order[led_idx];
59+
uint64_t tnow_us = *(uint64_t*)ctx;
60+
61+
struct profiLED_color_s ret;
62+
uint32_t period_us = 2000000;
63+
float t_sec = (float)(tnow_us%period_us) * 1e-6;
64+
// float t_rad = (float)(tnow_us%period_us)/period_us * 2*M_PI;
65+
66+
ret.r = ret.g = ret.b = 0;
67+
68+
if (led_strobe && (t_sec < 0.05 || (t_sec > 0.25 && t_sec < 0.3))) {
69+
ret.r = ret.g = ret.b = 255;
70+
}
71+
72+
return ret;
73+
}
74+
75+
static struct profiLED_color_s led_uavcan(uint32_t led_idx, void* ctx) {
76+
(void)led_idx;
77+
(void)ctx;
78+
return uavcan_commanded_color;
79+
}
80+
81+
static struct profiLED_color_s led_rainbow(uint32_t led_idx, void* ctx) {
82+
led_idx = board_led_order[led_idx];
83+
84+
uint64_t tnow_us = *(uint64_t*)ctx;
85+
float pos_rad = wrap_2pi(2*M_PI*(led_idx+0.5)/16.0-led_angle_param);
86+
87+
struct profiLED_color_s ret;
88+
uint32_t period_us = 1000000;
89+
float t_rad = (float)(tnow_us%period_us)/period_us * 2*M_PI;
90+
91+
ret.r = 25*(1+sinf(t_rad+pos_rad))/2;
92+
ret.g = 25*(1+sinf(t_rad+pos_rad+2*M_PI/3))/2;
93+
ret.b = 25*(1+sinf(t_rad+pos_rad+4*M_PI/3))/2;
94+
95+
return ret;
96+
}
97+
98+
static struct profiLED_color_s led_redgreen(uint32_t led_idx, void* ctx) {
99+
uint64_t tnow_us = *(uint64_t*)ctx;
100+
led_idx = board_led_order[led_idx];
101+
102+
float pos_rad = wrap_2pi(2*M_PI*(led_idx+0.5)/16.0-led_angle_param);
103+
104+
struct profiLED_color_s ret;
105+
uint32_t period_us = 2000000;
106+
float t_sec = (float)(tnow_us%period_us) * 1e-6;
107+
// float t_rad = (float)(tnow_us%period_us)/period_us * 2*M_PI;
108+
109+
ret.r = ret.g = ret.b = 0;
110+
111+
if (led_strobe && (t_sec < 0.05 || (t_sec > 0.25 && t_sec < 0.3))) {
112+
ret.r = ret.g = ret.b = 255;
113+
} else if (pos_rad < M_PI) {
114+
ret.g = 50;
115+
} else {
116+
ret.r = 50;
117+
}
118+
119+
return ret;
120+
}
121+
122+
static struct profiLED_color_s led_red(uint32_t led_idx, void* ctx) {
123+
led_idx = board_led_order[led_idx];
124+
uint64_t tnow_us = *(uint64_t*)ctx;
125+
// float pos_rad = wrap_2pi(2*M_PI*(led_idx+0.5)/16.0-led_angle_param);
126+
127+
struct profiLED_color_s ret;
128+
uint32_t period_us = 2000000;
129+
float t_sec = (float)(tnow_us%period_us) * 1e-6;
130+
// float t_rad = (float)(tnow_us%period_us)/period_us * 2*M_PI;
131+
132+
ret.g = ret.b = 0;
133+
ret.r = 50;
134+
135+
if (led_strobe && (t_sec < 0.05 || (t_sec > 0.25 && t_sec < 0.3))) {
136+
ret.r = ret.g = ret.b = 255;
137+
}
138+
139+
return ret;
140+
}
141+
142+
static struct profiLED_color_s led_green(uint32_t led_idx, void* ctx) {
143+
led_idx = board_led_order[led_idx];
144+
uint64_t tnow_us = *(uint64_t*)ctx;
145+
146+
struct profiLED_color_s ret;
147+
uint32_t period_us = 2000000;
148+
float t_sec = (float)(tnow_us%period_us) * 1e-6;
149+
// float t_rad = (float)(tnow_us%period_us)/period_us * 2*M_PI;
150+
151+
ret.r = ret.b = 0;
152+
ret.g = 50;
153+
154+
if (led_strobe && (t_sec < 0.05 || (t_sec > 0.25 && t_sec < 0.3))) {
155+
ret.r = ret.g = ret.b = 255;
156+
}
157+
158+
return ret;
159+
}
160+
161+
static struct profiLED_color_s led_police(uint32_t led_idx, void* ctx) {
162+
led_idx = board_led_order[led_idx];
163+
164+
uint64_t tnow_us = *(uint64_t*)ctx;
165+
float pos_rad = wrap_2pi(2*M_PI*(led_idx+0.5)/16.0-led_angle_param);
166+
167+
struct profiLED_color_s ret;
168+
uint32_t period_us = 1000000;
169+
170+
uint8_t step = 12*(tnow_us%period_us)/period_us;
171+
172+
ret.r = ret.g = ret.b = 0;
173+
174+
if (pos_rad < M_PI) {
175+
switch(step) {
176+
case 0:
177+
case 2:
178+
case 4:
179+
ret.b = 255;
180+
break;
181+
case 6:
182+
case 8:
183+
case 10:
184+
ret.r = ret.b = ret.g = 255;
185+
break;
186+
}
187+
} else {
188+
switch(step) {
189+
case 0:
190+
case 2:
191+
case 4:
192+
ret.r = ret.b = ret.g = 255;
193+
break;
194+
case 6:
195+
case 8:
196+
case 10:
197+
ret.r = 255;
198+
break;
199+
}
200+
}
201+
202+
return ret;
203+
}
204+
205+
static struct profiLED_color_s led_police_left(uint32_t led_idx, void* ctx) {
206+
led_idx = board_led_order[led_idx];
207+
208+
uint64_t tnow_us = *(uint64_t*)ctx;
209+
float pos_rad = wrap_2pi(2*M_PI*(led_idx+0.5)/16.0-led_angle_param);
210+
211+
struct profiLED_color_s ret;
212+
uint32_t period_us = 1000000;
213+
214+
uint8_t step = 12*(tnow_us%period_us)/period_us;
215+
216+
ret.r = ret.g = ret.b = 0;
217+
218+
switch(step) {
219+
case 0:
220+
case 2:
221+
case 4:
222+
ret.r = ret.b = ret.g = 255;
223+
break;
224+
case 6:
225+
case 8:
226+
case 10:
227+
ret.r = 255;
228+
break;
229+
}
230+
231+
return ret;
232+
}
233+
234+
static struct profiLED_color_s led_police_right(uint32_t led_idx, void* ctx) {
235+
led_idx = board_led_order[led_idx];
236+
237+
uint64_t tnow_us = *(uint64_t*)ctx;
238+
float pos_rad = wrap_2pi(2*M_PI*(led_idx+0.5)/16.0-led_angle_param);
239+
240+
struct profiLED_color_s ret;
241+
uint32_t period_us = 1000000;
242+
243+
uint8_t step = 12*(tnow_us%period_us)/period_us;
244+
245+
ret.r = ret.g = ret.b = 0;
246+
247+
switch(step) {
248+
case 0:
249+
case 2:
250+
case 4:
251+
ret.b = 255;
252+
break;
253+
case 6:
254+
case 8:
255+
case 10:
256+
ret.r = ret.b = ret.g = 255;
257+
break;
258+
}
259+
260+
return ret;
261+
}
262+
263+
static struct profiLED_color_s led_sequence(uint32_t led_idx, void* ctx) {
264+
led_idx = board_led_order[led_idx];
265+
266+
uint64_t tnow_us = *(uint64_t*)ctx;
267+
268+
struct profiLED_color_s ret;
269+
ret.r = ret.g = ret.b = 0;
270+
271+
uint32_t period_us = 20000000;
272+
273+
uint8_t step = 20*(tnow_us%period_us)/period_us;
274+
if (led_idx == step) {
275+
ret.r = ret.g = ret.b = 255;
276+
}
277+
return ret;
278+
}
279+
280+
281+
static struct profiLED_color_s led_heading(uint32_t led_idx, void* ctx) {
282+
// led_idx = board_led_order[led_idx];
283+
//
284+
// uint64_t tnow_us = *(uint64_t*)ctx;
285+
//
286+
// uint32_t period_us = 60000000;
287+
//
288+
// float pos_rad = wrap_2pi(2*M_PI*(led_idx+0.5)/16.0-led_angle_param);
289+
//
290+
// if (!heading_valid) {
291+
// struct profiLED_color_s ret;
292+
// ret.b = ret.g = 0;
293+
// ret.r = 10;
294+
// return ret;
295+
// }
296+
//
297+
// float heading = ((tnow_us%period_us) / (float)period_us)*2*M_PI;
298+
//
299+
// float dist = fabsf(wrap_pi(-heading-pos_rad))/M_PI;
300+
//
301+
// float intensity = 1-dist*8;
302+
// if (intensity < 0) {
303+
// intensity = 0;
304+
// }
305+
//
306+
struct profiLED_color_s ret;
307+
ret.r = ret.g = ret.b = 0;
308+
return ret;
309+
}
310+
311+
static void timer_task_func(struct worker_thread_timer_task_s* task) {
312+
(void)task;
313+
314+
switch(led_power_mode_param) {
315+
case 1:
316+
palSetLine(BOARD_PAL_LINE_LED_ENABLE_1);
317+
palClearLine(BOARD_PAL_LINE_LED_ENABLE_2);
318+
break;
319+
case 2:
320+
palSetLine(BOARD_PAL_LINE_LED_ENABLE_1);
321+
palSetLine(BOARD_PAL_LINE_LED_ENABLE_2);
322+
break;
323+
default:
324+
case 0:
325+
palClearLine(BOARD_PAL_LINE_LED_ENABLE_1);
326+
palClearLine(BOARD_PAL_LINE_LED_ENABLE_2);
327+
break;
328+
}
329+
330+
uint64_t tnow_us = uavcan_timesync_get_bus_time_now();
331+
if (tnow_us == 0) {
332+
tnow_us = micros64();
333+
}
334+
335+
uint32_t t_begin_us = micros();
336+
337+
uint8_t led_mode = led_mode_param;
338+
339+
if (led_mode >= NUM_LED_MODES) {
340+
led_mode = 0;
341+
}
342+
343+
profiLED_output_spi(&led_spi, 16, led_mode_funcs[led_mode], &tnow_us);
344+
345+
int32_t time_error_us = (int32_t)(tnow_us%16384) - 16384/2;
346+
uint32_t time_correction = (micros()-t_begin_us) + time_error_us;
347+
348+
if (time_correction > 16384) {
349+
time_correction = 16384;
350+
}
351+
352+
worker_thread_timer_task_reschedule(&led_thread, &timer_task, chTimeUS2I(16384-time_correction));
353+
}
354+
355+
static void led_command_handler(size_t msg_size, const void* buf, void* ctx)
356+
{
357+
(void)msg_size;
358+
(void)ctx;
359+
const struct uavcan_deserialized_message_s* msg_wrapper = buf;
360+
const struct uavcan_equipment_indication_LightsCommand_s* msg = (const struct uavcan_equipment_indication_LightsCommand_s*)msg_wrapper->msg;
361+
uavcan_commanded_color.r = msg->commands[0].color.red*8;
362+
uavcan_commanded_color.g = msg->commands[0].color.green*4;
363+
uavcan_commanded_color.b = msg->commands[0].color.blue*8;
364+
}

‎bootloader/src/setup.c

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -5,3 +5,4 @@ PUBSUB_TOPIC_GROUP_CREATE(default_topic_group, 16384)
55

66
WORKER_THREAD_TAKEOVER_MAIN(lpwork_thread, LOWPRIO+1)
77
WORKER_THREAD_SPAWN(can_thread, LOWPRIO+2, 2048)
8+
WORKER_THREAD_SPAWN(led_thread, LOWPRIO+3, 2048)

0 commit comments

Comments
 (0)