Implemented fading as it was in the AVR implementation

Makefile

@@ -11,7 +11,7 @@ BUILD:=debug
 
 # basic build flags configuration
 CFLAGS+=-std=c99 -Wall -pedantic -Wno-long-long -D_POSIX_C_SOURCE=20120607L -D_FILE_OFFSET_BITS=64
-LIBS+=-lm
+LIBS+=-lm -lrt
 
 # library specific flags
 # wiringPi

include/fader.h

@@ -0,0 +1,15 @@
+#ifndef FADER_H
+#define FADER_H
+
+#include <stdint.h>
+
+int  fader_init(uint8_t nMod);
+void fader_shutdown(void);
+void fader_set_colour(uint8_t module, uint8_t r, uint8_t g, uint8_t b);
+void fader_fade_colour(uint8_t module, uint8_t r, uint8_t g, uint8_t b);
+void fader_add_colour(uint8_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

include/udpproto.h

@@ -0,0 +1,10 @@
+#ifndef UDPPROTO_H
+#define UDPPROTO_H
+
+#include <stdint.h>
+
+int udpproto_init(uint16_t port);
+int udpproto_process(void);
+void udpproto_shutdown(void);
+
+#endif // UDPPROTO_H

include/utils.h

@@ -0,0 +1,18 @@
+/*
+ * vim: sw=2 ts=2 expandtab
+ *
+ * "THE PIZZA-WARE LICENSE" (derived from "THE BEER-WARE LICENCE"):
+ * Thomas Kolb <cfr34k@tkolb.de> 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

src/fader.c

@@ -0,0 +1,137 @@
+#include <malloc.h>
+
+#include "../include/logger.h"
+#include "../include/utils.h"
+#include "../include/ws2801.h"
+
+#include "../include/fader.h"
+
+uint8_t numModules;
+
+float fadestep = 1;
+double nextFrame;
+
+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(uint8_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(uint8_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(uint8_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;
+}
+
+void fader_fade_colour(uint8_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(uint8_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; }
+}
+
+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;
+}
+
+void fade_colour(float *cur, const float *target)
+{
+	float diff;
+	if(*cur > *target) {
+		diff = *cur - *target;
+		if(diff < fadestep) {
+			*cur = *target;
+		} else {
+			*cur -= fadestep;
+		}
+	} else if(*cur < *target) {
+		diff = *target - *cur;
+		if(diff < fadestep) {
+			*cur = *target;
+		} else {
+			*cur += fadestep;
+		}
+	}
+}
+
+void fader_update(void)
+{
+	for(uint8_t i = 0; i < numModules; i++) {
+		fade_colour(&(curColour[i].red), &(targetColour[i].red));
+		fade_colour(&(curColour[i].green), &(targetColour[i].green));
+		fade_colour(&(curColour[i].blue), &(targetColour[i].blue));
+
+		ws2801_set_colour(i,
+				(uint8_t)curColour[i].red,
+				(uint8_t)curColour[i].green,
+				(uint8_t)curColour[i].blue);
+	}
+
+	ws2801_send_update();
+}
+
+void fader_wait_frame(void)
+{
+	sleep_until(nextFrame);
+	nextFrame += interval;
+}

src/main.c

@@ -5,51 +5,23 @@
 #include <errno.h>
 #include <stdint.h>
 
-#include <arpa/inet.h>
-#include <netinet/in.h>
-#include <sys/types.h>
-#include <sys/socket.h>
-#include <unistd.h>
-
 #include "../include/ws2801.h"
 #include "../include/logger.h"
+#include "../include/fader.h"
+#include "../include/udpproto.h"
 
 #define PORT 2703
 
 #define NUM_MODULES 20
 
-#define SET_COLOUR   0
-#define FADE_COLOUR  1
-#define ADD_COLOUR   2
-#define SET_FADESTEP 3
-
 int main(void)
 {
-	int sock;
-	socklen_t remote_addr_len;
-	struct sockaddr_in listen_addr, remote_addr;
-	uint8_t pkgbuf[256];
-	ssize_t rcvbytes, offset = 0;
-
-	uint8_t cmd, r, g, b, action, module;
-
 	// initialize logger	
 	logger_init();
 
-	// initialize UDP server socket
+	// initialise the UDP server
-	sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
+	if(udpproto_init(PORT) == -1) {
-	if(sock == -1) {
+		LOG(LVL_FATAL, "Could not initialize the UDP server.");
-		LOG(LVL_FATAL, "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_FATAL, "Could not bind socket to port %i: %s.", PORT, strerror(errno));
 		return 1;
 	}
 
@@ -59,46 +31,24 @@ int main(void)
 		return 1;
 	}
 
-	LOG(LVL_INFO, "Initialisation complete.");
+	// initialise the LED fader
-
+	if(fader_init(NUM_MODULES) == -1) {
-
+		LOG(LVL_FATAL, "Could not initialize the LED fader.");
-	while(1) {
+		return 1;
-		remote_addr_len = sizeof(remote_addr);
-		rcvbytes = recvfrom(sock, pkgbuf, 256, 0, (struct sockaddr*)&remote_addr, &remote_addr_len);
-		if(rcvbytes == -1) {
-			LOG(LVL_ERR, "recvfrom() failed: %s. Shutting down.", strerror(errno));
-			break;
-		}
-
-		pkgbuf[rcvbytes] = 0;
-
-		// commands from packet
-		offset = 0;
-		while(offset <= rcvbytes - 3) {
-			cmd = pkgbuf[offset + 0];
-			r   = pkgbuf[offset + 1];
-			g   = pkgbuf[offset + 2];
-			b   = pkgbuf[offset + 3];
-			offset += 4;
-
-			action = (cmd & 0xC0) >> 6; // the upper 2 bits
-			module = cmd & 0x3F; // the lower 6 bits
-
-			if(action != SET_COLOUR) {
-				// action not implemented yet -> ignore
-				LOG(LVL_DEBUG, "Action %u not implemented yet.", action);
-				continue;
-			}
-
-			ws2801_set_colour(module, r, g, b);
-		}
-
-		ws2801_send_update();
 	}
 
-	close(sock);
+	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;
 }

src/udpproto.c

@@ -0,0 +1,122 @@
+#include <arpa/inet.h>
+#include <netinet/in.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <unistd.h>
+#include <errno.h>
+#include <string.h>
+#include <poll.h>
+
+#include "../include/logger.h"
+#include "../include/fader.h"
+
+#include "../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[256];
+	ssize_t rcvbytes, offset = 0;
+
+	uint8_t cmd, 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, 256, 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 - 3) {
+		cmd = pkgbuf[offset + 0];
+		r   = pkgbuf[offset + 1];
+		g   = pkgbuf[offset + 2];
+		b   = pkgbuf[offset + 3];
+		offset += 4;
+
+		action = (cmd & 0xC0) >> 6; // the upper 2 bits
+		module = cmd & 0x3F; // the lower 6 bits
+
+		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 / 4; // number of commands in packet
+}
+
+void udpproto_shutdown(void)
+{
+	close(sock);
+}

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 <cfr34k@tkolb.de> 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 <errno.h>
+
+#include <time.h>
+#include <stdint.h>
+
+#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);
+}
+