commit f033119aa80c344a28ca9df412afabf57c7be35a Author: Thomas Kolb Date: Wed Jun 13 21:51:30 2018 +0200 Initial import from ws2801d repository diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..567609b --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +build/ diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..c78a03d --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_minimum_required (VERSION 3.2) +project (sk6812d VERSION 0.1 LANGUAGES C) + +set(CMAKE_C_STANDARD 99) +set(CMAKE_CXX_FLAGS "-Wall -pedantic") + +find_package(Threads REQUIRED) + +# put all .cpp and .h files into the sources variable +set(sources + src/fader.c + src/logger.c + src/main.c + src/udpproto.c + src/utils.c + src/ws2801.c + src/fader.h + src/logger.h + src/udpproto.h + src/utils.h + src/ws2801.h + ) + +include_directories( + ${CMAKE_PROJECT_NAME} + ) + +add_executable( + ${CMAKE_PROJECT_NAME} + ${sources} + ) + +target_link_libraries( + ${CMAKE_PROJECT_NAME} + ${CMAKE_THREAD_LIBS_INIT} + ) + +#configure_file("lua/demobot.lua" "lua/demobot.lua" COPYONLY) diff --git a/make.sh b/make.sh new file mode 100755 index 0000000..4434f0e --- /dev/null +++ b/make.sh @@ -0,0 +1,6 @@ +#!/bin/sh + +mkdir -p build +cd build +cmake .. +make $@ diff --git a/src/fader.c b/src/fader.c new file mode 100644 index 0000000..f68099d --- /dev/null +++ b/src/fader.c @@ -0,0 +1,157 @@ +#include + +#include "logger.h" +#include "utils.h" +#include "ws2801.h" + +#include "fader.h" + +uint32_t numModules; + +float fadestep = 1; +double nextFrame; + +int somethingChanged = 0; // indicates when a ws2801 update is required + +static const double interval = 0.01f; + +struct Colour { + float red, green, blue; // value range is 0.0 to 255.0 +}; + +struct Colour *curColour; +struct Colour *targetColour; + +int fader_init(uint32_t nMod) +{ + numModules = nMod; + + curColour = malloc(nMod * sizeof(struct Colour)); + if(!curColour) { + LOG(LVL_ERR, "fader: could not allocate the array of current colours!"); + return -1; + } + + targetColour = malloc(nMod * sizeof(struct Colour)); + if(!targetColour) { + LOG(LVL_ERR, "fader: could not allocate the array of target colours!"); + return -1; + } + + for(uint32_t i = 0; i < numModules; i++) { + curColour[i].red = targetColour[i].red = 0; + curColour[i].green = targetColour[i].green = 0; + curColour[i].blue = targetColour[i].blue = 0; + } + + // timestamp for the first frame + nextFrame = get_hires_time() + interval; + + return 0; +} + +void fader_shutdown(void) +{ + free(curColour); + free(targetColour); +} + +void fader_set_colour(uint32_t module, uint8_t r, uint8_t g, uint8_t b) +{ + curColour[module].red = targetColour[module].red = r; + curColour[module].green = targetColour[module].green = g; + curColour[module].blue = targetColour[module].blue = b; + + somethingChanged = 1; +} + +void fader_fade_colour(uint32_t module, uint8_t r, uint8_t g, uint8_t b) +{ + targetColour[module].red = r; + targetColour[module].green = g; + targetColour[module].blue = b; +} + +void fader_add_colour(uint32_t module, uint8_t r, uint8_t g, uint8_t b) +{ + curColour[module].red += r; + curColour[module].green += g; + curColour[module].blue += b; + + if(curColour[module].red > 255) { curColour[module].red = 255; } + if(curColour[module].green > 255) { curColour[module].green = 255; } + if(curColour[module].blue > 255) { curColour[module].blue = 255; } + + targetColour[module].red += r; + targetColour[module].green += g; + targetColour[module].blue += b; + + if(targetColour[module].red > 255) { targetColour[module].red = 255; } + if(targetColour[module].green > 255) { targetColour[module].green = 255; } + if(targetColour[module].blue > 255) { targetColour[module].blue = 255; } + + somethingChanged = 1; +} + +void fader_set_fadestep(uint8_t newFadestep) +{ + // The original avr implementition had a frame rate of 25fps (interval 0,04 sec.). + // This scales the fadestep to the current frame rate. + fadestep = (float)newFadestep * interval / 0.04f; +} + +/*! + * Fade the colour value in cur towards target. + * + * \param cur The colour value to update. + * \param target The target value that should be reached. + * \param changed Output value which is set to 1 if cur was changed. + */ +void fade_colour(float *cur, const float *target, int *changed) +{ + float diff; + if(*cur > *target) { + diff = *cur - *target; + if(diff < fadestep) { + *cur = *target; + } else { + *cur -= fadestep; + } + + *changed = 1; + } else if(*cur < *target) { + diff = *target - *cur; + if(diff < fadestep) { + *cur = *target; + } else { + *cur += fadestep; + } + + *changed = 1; + } +} + +void fader_update(void) +{ + for(uint32_t i = 0; i < numModules; i++) { + fade_colour(&(curColour[i].red), &(targetColour[i].red), &somethingChanged); + fade_colour(&(curColour[i].green), &(targetColour[i].green), &somethingChanged); + fade_colour(&(curColour[i].blue), &(targetColour[i].blue), &somethingChanged); + + ws2801_set_colour(i, + (uint8_t)curColour[i].red, + (uint8_t)curColour[i].green, + (uint8_t)curColour[i].blue); + } + + if(somethingChanged) { + ws2801_send_update(); + somethingChanged = 0; + } +} + +void fader_wait_frame(void) +{ + sleep_until(nextFrame); + nextFrame += interval; +} diff --git a/src/fader.h b/src/fader.h new file mode 100644 index 0000000..f729161 --- /dev/null +++ b/src/fader.h @@ -0,0 +1,15 @@ +#ifndef FADER_H +#define FADER_H + +#include + +int fader_init(uint32_t nMod); +void fader_shutdown(void); +void fader_set_colour(uint32_t module, uint8_t r, uint8_t g, uint8_t b); +void fader_fade_colour(uint32_t module, uint8_t r, uint8_t g, uint8_t b); +void fader_add_colour(uint32_t module, uint8_t r, uint8_t g, uint8_t b); +void fader_set_fadestep(uint8_t newFadestep); +void fader_update(void); +void fader_wait_frame(void); + +#endif // FADER_H diff --git a/src/logger.c b/src/logger.c new file mode 100644 index 0000000..2618482 --- /dev/null +++ b/src/logger.c @@ -0,0 +1,161 @@ +/* + * vim: sw=2 ts=2 expandtab + * + * "THE PIZZA-WARE LICENSE" (derived from "THE BEER-WARE LICENCE"): + * Thomas Kolb wrote this file. As long as you retain this + * notice you can do whatever you want with this stuff. If we meet some day, + * and you think this stuff is worth it, you can buy me a pizza in return. + * - Thomas Kolb + */ + +#include +#include + +#include +#include + +#include "logger.h" + +// define constants +const char *LOGGER_STR_FATAL = "FATAL"; +const char *LOGGER_STR_ERR = "ERROR"; +const char *LOGGER_STR_WARN = "WARN "; +const char *LOGGER_STR_INFO = "INFO "; +const char *LOGGER_STR_DEBUG = "DEBUG"; +const char *LOGGER_STR_DUMP = "DUMP "; + +const char *LOGGER_COLOR_FATAL = "\033[1;31m"; +const char *LOGGER_COLOR_ERR = "\033[1;31m"; +const char *LOGGER_COLOR_WARN = "\033[1;33m"; +const char *LOGGER_COLOR_INFO = "\033[1;32m"; +const char *LOGGER_COLOR_DEBUG = "\033[1m"; +const char *LOGGER_COLOR_DUMP = "\033[1;30m"; +const char *LOGGER_COLOR_NONE = "\033[0m"; + +// global variables +sem_t logger_semaphore; +int logger_verbosity; +int logger_use_colors; + +void logger_init(void) { + // Initialize the semaphore + sem_init(&logger_semaphore, 0, 1); + + logger_verbosity = 2147483647; + logger_use_colors = 1; +} + +void logger_shutdown(void) { + sem_destroy(&logger_semaphore); +} + +void logger_enable_colors(int enable) { + logger_use_colors = enable; +} + +void logger_set_verbosity(int verbosity) { + logger_verbosity = verbosity; +} + +void logger_debug_message(const char *prefix, const char *fmt, va_list ap) { + /* Guess we need no more than 100 bytes. */ + int n, size = 100; + char *p, *np; + va_list internal_ap; + + if ((p = (char*)malloc(size)) == NULL) { + fprintf(stderr, "[%s] FATAL: Cannot allocate string buffer while processing arguments.\n", LOGGER_STR_ERR); + return; + } + + while (1) { + /* Try to print in the allocated space. */ + va_copy(internal_ap, ap); + n = vsnprintf(p, size, fmt, internal_ap); + va_end(internal_ap); + + /* If that worked, return the string. */ + if (n > -1 && n < size) + break; + + /* Else try again with more space. */ + if (n > -1) /* glibc 2.1 */ + size = n+1; /* precisely what is needed */ + else /* glibc 2.0 */ + size *= 2; /* twice the old size */ + + if ((np = (char*)realloc (p, size)) == NULL) { + free(p); + fprintf(stderr, "[%s] FATAL: Cannot reallocate string buffer while processing arguments.\n", LOGGER_STR_ERR); + return; + } else { + p = np; + } + } + + sem_wait(&logger_semaphore); + fprintf(stderr, "%s %s\n", prefix, p); + sem_post(&logger_semaphore); + + free(p); +} + +void logger_log(int level, const char *format, ...) { + va_list argptr; + + char timebuf[32]; + char timebuf2[32]; + + char prefixbuf[64]; + const char *prefixcolor = "", *prefixtext = ""; + + if(level > logger_verbosity) + return; + + struct timeval tv; + gettimeofday(&tv, NULL); + strftime(timebuf, 32, "%Y-%M-%d %H:%M:%S.%%03d", localtime(&(tv.tv_sec))); + snprintf(timebuf2, 32, timebuf, tv.tv_usec/1000); + + if(level >= LVL_DUMP) { + if(logger_use_colors) + prefixcolor = LOGGER_COLOR_DUMP; + + prefixtext = LOGGER_STR_DUMP; + } else if(level >= LVL_DEBUG) { + if(logger_use_colors) + prefixcolor = LOGGER_COLOR_DEBUG; + + prefixtext = LOGGER_STR_DEBUG; + } else if(level >= LVL_INFO) { + if(logger_use_colors) + prefixcolor = LOGGER_COLOR_INFO; + + prefixtext = LOGGER_STR_INFO; + } else if(level >= LVL_WARN) { + if(logger_use_colors) + prefixcolor = LOGGER_COLOR_WARN; + + prefixtext = LOGGER_STR_WARN; + } else if(level >= LVL_ERR) { + if(logger_use_colors) + prefixcolor = LOGGER_COLOR_ERR; + + prefixtext = LOGGER_STR_ERR; + } else { + if(logger_use_colors) + prefixcolor = LOGGER_COLOR_FATAL; + + prefixtext = LOGGER_STR_FATAL; + } + + if(logger_use_colors) { + sprintf(prefixbuf, "%s [%s%s%s]", timebuf2, prefixcolor, prefixtext, LOGGER_COLOR_NONE); + } else { + sprintf(prefixbuf, "%s [%s]", timebuf2, prefixtext); + } + + va_start(argptr, format); + logger_debug_message(prefixbuf, format, argptr); + va_end(argptr); +} diff --git a/src/logger.h b/src/logger.h new file mode 100644 index 0000000..844175f --- /dev/null +++ b/src/logger.h @@ -0,0 +1,37 @@ +/* + * vim: sw=2 ts=2 expandtab + * + * "THE PIZZA-WARE LICENSE" (derived from "THE BEER-WARE LICENCE"): + * Thomas Kolb wrote this file. As long as you retain this + * notice you can do whatever you want with this stuff. If we meet some day, + * and you think this stuff is worth it, you can buy me a pizza in return. + * - Thomas Kolb + */ + +#ifndef LOGGER_H +#define LOGGER_H + +#include +#include + +static const int LVL_FATAL = 0; /*!< Fatal message level */ +static const int LVL_ERR = 5; /*!< Error message level */ +static const int LVL_WARN = 50; /*!< Warning message level */ +static const int LVL_INFO = 100; /*!< Information message level */ +static const int LVL_DEBUG = 200; /*!< Debug message level */ +static const int LVL_DUMP = 500; /*!< Dump message level */ + +extern sem_t logger_semaphore; +extern int logger_verbosity; +extern int logger_use_colors; + +void logger_init(void); +void logger_shutdown(void); +void logger_enable_colors(int enable); +void logger_set_verbosity(int verbosity); + +void logger_log(int level, const char *format, ...); + +#define LOG(level, ...) logger_log(level, __VA_ARGS__) + +#endif // LOGGER_H diff --git a/src/main.c b/src/main.c new file mode 100644 index 0000000..1deffa6 --- /dev/null +++ b/src/main.c @@ -0,0 +1,54 @@ +#include + +#include +#include +#include +#include + +#include "ws2801.h" +#include "logger.h" +#include "fader.h" +#include "udpproto.h" + +#define PORT 2703 + +#define NUM_MODULES 160 + +int main(void) +{ + // initialize logger + logger_init(); + + // initialise the UDP server + if(udpproto_init(PORT) == -1) { + LOG(LVL_FATAL, "Could not initialize the UDP server."); + return 1; + } + + // initialize ws2801 library + if(ws2801_init(NUM_MODULES) == -1) { + LOG(LVL_FATAL, "Could not initialize WS2801 library."); + return 1; + } + + // initialise the LED fader + if(fader_init(NUM_MODULES) == -1) { + LOG(LVL_FATAL, "Could not initialize the LED fader."); + return 1; + } + + LOG(LVL_INFO, "Initialisation complete."); + + while(1) { + udpproto_process(); + fader_update(); + fader_wait_frame(); + } + + // shut down all modules + fader_shutdown(); + ws2801_shutdown(); + udpproto_shutdown(); + + return 0; +} diff --git a/src/udpproto.c b/src/udpproto.c new file mode 100644 index 0000000..0899b50 --- /dev/null +++ b/src/udpproto.c @@ -0,0 +1,120 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include "logger.h" +#include "fader.h" + +#include "udpproto.h" + +#define SET_COLOUR 0 +#define FADE_COLOUR 1 +#define ADD_COLOUR 2 +#define SET_FADESTEP 3 + +int sock; + +int udpproto_init(uint16_t port) +{ + struct sockaddr_in listen_addr; + + // initialize UDP server socket + sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); + if(sock == -1) { + LOG(LVL_ERR, "udpproto: Could not initialize UDP socket: %s.", strerror(errno)); + return -1; + } + + memset(&listen_addr, 0, sizeof(listen_addr)); + listen_addr.sin_family = AF_INET; + listen_addr.sin_port = htons(port); + listen_addr.sin_addr.s_addr = htonl(INADDR_ANY); + + if(bind(sock, (struct sockaddr*)&listen_addr, sizeof(listen_addr)) == -1) { + LOG(LVL_ERR, "udpproto: Could not bind socket to port %i: %s.", port, strerror(errno)); + return -1; + } + + return 0; +} + +int udpproto_process(void) +{ + socklen_t remote_addr_len; + struct sockaddr remote_addr; + uint8_t pkgbuf[65536]; + ssize_t rcvbytes, offset = 0; + + uint8_t r, g, b, action, module; + int fds_ready; + + // check if there is data to be read (to prevent blocking) + struct pollfd pfd = { + .fd = sock, + .events = POLLIN, + .revents = 0 + }; + + fds_ready = poll(&pfd, 1, 0); + if(fds_ready == -1) { + LOG(LVL_ERR, "udpproto: poll() failed: %s.", strerror(errno)); + return -1; + } else if(fds_ready == 0) { + // there's nothing to be read + return 0; + } + + // receive the data + remote_addr_len = sizeof(remote_addr); + rcvbytes = recvfrom(sock, pkgbuf, 65536, 0, (struct sockaddr*)&remote_addr, &remote_addr_len); + if(rcvbytes == -1) { + LOG(LVL_ERR, "udpproto: recvfrom() failed: %s.", strerror(errno)); + return -1; + } + + pkgbuf[rcvbytes] = 0; + + // parse commands from packet + offset = 0; + while(offset <= rcvbytes - 4) { + action = pkgbuf[offset + 0]; + module = pkgbuf[offset + 1]; + r = pkgbuf[offset + 2]; + g = pkgbuf[offset + 3]; + b = pkgbuf[offset + 4]; + offset += 5; + + switch(action) { + case SET_COLOUR: + fader_set_colour(module, r, g, b); + break; + + case FADE_COLOUR: + fader_fade_colour(module, r, g, b); + break; + + case ADD_COLOUR: + fader_add_colour(module, r, g, b); + break; + + case SET_FADESTEP: + fader_set_fadestep(r); // red channel contains the fadestep in this case + break; + + default: + LOG(LVL_DEBUG, "udpproto: Action %u not implemented yet.", action); + } + } + + return rcvbytes / 5; // number of commands in packet +} + +void udpproto_shutdown(void) +{ + close(sock); +} diff --git a/src/udpproto.h b/src/udpproto.h new file mode 100644 index 0000000..3f296a8 --- /dev/null +++ b/src/udpproto.h @@ -0,0 +1,10 @@ +#ifndef UDPPROTO_H +#define UDPPROTO_H + +#include + +int udpproto_init(uint16_t port); +int udpproto_process(void); +void udpproto_shutdown(void); + +#endif // UDPPROTO_H diff --git a/src/utils.c b/src/utils.c new file mode 100644 index 0000000..737f4fb --- /dev/null +++ b/src/utils.c @@ -0,0 +1,43 @@ +/* + * vim: sw=2 ts=2 expandtab + * + * "THE PIZZA-WARE LICENSE" (derived from "THE BEER-WARE LICENCE"): + * Thomas Kolb wrote this file. As long as you retain this + * notice you can do whatever you want with this stuff. If we meet some day, + * and you think this stuff is worth it, you can buy me a pizza in return. + * - Thomas Kolb + */ + +#include + +#include +#include + +#include "utils.h" + +double get_hires_time(void) { + struct timespec clk; + clock_gettime(CLOCK_REALTIME, &clk); + return (double)clk.tv_sec + 1.0e-9f * clk.tv_nsec; +} + +void fsleep(double d) { + struct timespec ts; + + ts.tv_sec = (time_t)d; + ts.tv_nsec = (long)(1e9 * (d - (long)d)); + + nanosleep(&ts, NULL); +} + +void sleep_until(double hires_time) { + struct timespec tv; + int ret; + + tv.tv_sec = hires_time; + tv.tv_nsec = (uint64_t)(1e9 * hires_time) % 1000000000; + do { + ret = clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &tv, NULL); + } while(ret == EINTR); +} + diff --git a/src/utils.h b/src/utils.h new file mode 100644 index 0000000..cba7fd3 --- /dev/null +++ b/src/utils.h @@ -0,0 +1,18 @@ +/* + * vim: sw=2 ts=2 expandtab + * + * "THE PIZZA-WARE LICENSE" (derived from "THE BEER-WARE LICENCE"): + * Thomas Kolb wrote this file. As long as you retain this + * notice you can do whatever you want with this stuff. If we meet some day, + * and you think this stuff is worth it, you can buy me a pizza in return. + * - Thomas Kolb + */ + +#ifndef UTILS_H +#define UTILS_H + +double get_hires_time(void); +void fsleep(double d); +void sleep_until(double hires_time); + +#endif // UTILS_H diff --git a/src/ws2801.c b/src/ws2801.c new file mode 100644 index 0000000..89ac550 --- /dev/null +++ b/src/ws2801.c @@ -0,0 +1,107 @@ +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +//#include + +#include "logger.h" +#include "ws2801.h" + +static const char *spi_dev = "/dev/spidev0.0"; +static const uint32_t spi_speed = 1000000; // clock freq in Hz +static const uint16_t spi_delay = 0; // us +static const uint8_t spi_bits = 8; // bits per word + +uint32_t numModules; +uint8_t *message; + +int spi_fd = 0; + +void send_message(void) +{ + struct spi_ioc_transfer tr = { + .tx_buf = (unsigned long)message, + .rx_buf = (unsigned long)NULL, + .len = 3 * numModules, + .delay_usecs = spi_delay, + .speed_hz = spi_speed, + .bits_per_word = 8 + }; + + if(ioctl(spi_fd, SPI_IOC_MESSAGE(1), &tr) < 0) { + LOG(LVL_ERR, "ws2801: could not send SPI message: %s", strerror(errno)); + } +} + +int ws2801_init(uint32_t nMod) +{ + // Initialize SPI + // We need to have stable data at falling edge of the spi interface (this + // means rising edge at the led strip, as the signal is inverted. + uint8_t spi_mode = SPI_NO_CS; // | SPI_CPOL; + + spi_fd = open(spi_dev, O_RDWR); + if(spi_fd < 0) { + LOG(LVL_ERR, "ws2801: cannot open %s: %s", spi_dev, strerror(errno)); + return -1; + } + + if(ioctl(spi_fd, SPI_IOC_WR_MODE, &spi_mode) == -1) { + LOG(LVL_ERR, "ws2801: cannot change SPI mode: %s", strerror(errno)); + return -1; + } + + if(ioctl(spi_fd, SPI_IOC_WR_BITS_PER_WORD, &spi_bits) == -1) { + LOG(LVL_ERR, "ws2801: cannot change SPI bits per word: %s", strerror(errno)); + return -1; + } + + if(ioctl(spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed) == -1) { + LOG(LVL_ERR, "ws2801: cannot change SPI speed: %s", strerror(errno)); + return -1; + } + + // create the message array + numModules = nMod; + message = malloc(3 * nMod * sizeof(uint8_t)); + + if(!message) { + LOG(LVL_ERR, "ws2801: could not allocate the message array!"); + return -1; + } + + return 0; +} + +void ws2801_shutdown(void) +{ + close(spi_fd); + + if(message != NULL) { free(message); message = NULL; } +} + +void ws2801_set_colour(uint32_t module, uint8_t red, uint8_t green, uint8_t blue) +{ + //message[3*module + 0] = ~red; + //message[3*module + 1] = ~green; + //message[3*module + 2] = ~blue; + message[3*module + 0] = red; + message[3*module + 1] = blue; + message[3*module + 2] = green; +} + +void ws2801_send_update(void) +{ + send_message(); + + static const struct timespec sleepval = {0, 550000}; + nanosleep(&sleepval, NULL); +} + diff --git a/src/ws2801.h b/src/ws2801.h new file mode 100644 index 0000000..3791842 --- /dev/null +++ b/src/ws2801.h @@ -0,0 +1,11 @@ +#ifndef WS2801_H +#define WS2801_H + +#include + +int ws2801_init(uint32_t num_modules); +void ws2801_shutdown(void); +void ws2801_set_colour(uint32_t module, uint8_t red, uint8_t green, uint8_t blue); +void ws2801_send_update(void); + +#endif // WS2801_H