92 lines
2.0 KiB
C
92 lines
2.0 KiB
C
/*
|
|
* Copyright (c) 2019 Vestas Wind Systems A/S
|
|
*
|
|
* SPDX-License-Identifier: Apache-2.0
|
|
*/
|
|
|
|
#include <CANopen.h>
|
|
#include <canbus/canopen.h>
|
|
|
|
struct canopen_leds_state {
|
|
CO_NMT_t *nmt;
|
|
canopen_led_callback_t green_cb;
|
|
void *green_arg;
|
|
canopen_led_callback_t red_cb;
|
|
void *red_arg;
|
|
bool green : 1;
|
|
bool red : 1;
|
|
};
|
|
|
|
static struct canopen_leds_state canopen_leds;
|
|
|
|
static void canopen_leds_update(struct k_timer *timer_id)
|
|
{
|
|
bool green = false;
|
|
bool red = false;
|
|
|
|
ARG_UNUSED(timer_id);
|
|
|
|
CO_NMT_blinkingProcess50ms(canopen_leds.nmt);
|
|
|
|
green = LED_GREEN_RUN(canopen_leds.nmt);
|
|
red = LED_RED_ERROR(canopen_leds.nmt);
|
|
|
|
#ifdef CONFIG_CANOPEN_LEDS_BICOLOR
|
|
if (red && canopen_leds.red_cb) {
|
|
green = false;
|
|
}
|
|
#endif
|
|
|
|
if (canopen_leds.green_cb) {
|
|
if (green != canopen_leds.green) {
|
|
canopen_leds.green_cb(green, canopen_leds.green_arg);
|
|
canopen_leds.green = green;
|
|
}
|
|
}
|
|
|
|
if (canopen_leds.red_cb) {
|
|
if (red != canopen_leds.red) {
|
|
canopen_leds.red_cb(red, canopen_leds.red_arg);
|
|
canopen_leds.red = red;
|
|
}
|
|
}
|
|
}
|
|
|
|
K_TIMER_DEFINE(canopen_leds_timer, canopen_leds_update, NULL);
|
|
|
|
void canopen_leds_init(CO_NMT_t *nmt,
|
|
canopen_led_callback_t green_cb, void *green_arg,
|
|
canopen_led_callback_t red_cb, void *red_arg)
|
|
{
|
|
k_timer_stop(&canopen_leds_timer);
|
|
|
|
canopen_leds.nmt = nmt;
|
|
|
|
/* Call existing callbacks to turn off LEDs */
|
|
if (canopen_leds.green_cb) {
|
|
canopen_leds.green_cb(false, canopen_leds.green_arg);
|
|
}
|
|
if (canopen_leds.red_cb) {
|
|
canopen_leds.red_cb(false, canopen_leds.red_arg);
|
|
}
|
|
|
|
canopen_leds.green_cb = green_cb;
|
|
canopen_leds.green_arg = green_arg;
|
|
canopen_leds.green = false;
|
|
canopen_leds.red_cb = red_cb;
|
|
canopen_leds.red_arg = red_arg;
|
|
canopen_leds.red = false;
|
|
|
|
/* Call new callbacks to turn off LEDs */
|
|
if (canopen_leds.green_cb) {
|
|
canopen_leds.green_cb(false, canopen_leds.green_arg);
|
|
}
|
|
if (canopen_leds.red_cb) {
|
|
canopen_leds.red_cb(false, canopen_leds.red_arg);
|
|
}
|
|
|
|
if (nmt && (green_cb || red_cb)) {
|
|
k_timer_start(&canopen_leds_timer, K_MSEC(50), K_MSEC(50));
|
|
}
|
|
}
|