TauLabs/PicoC: Ansteuerung RGB LEDs WS2812

#include "system.h"
#include "gpsposition.h"
#include "flightbatterystate.h"
#include "flightstatus.h"






/* WS2812 library */
#define nLED 3
unsigned char bufLED[nLED*3];


void setLED(unsigned short n, unsigned char r, unsigned char g, unsigned char b) {
	if (n < nLED) {
		unsigned short offset = 3*n;
		bufLED[offset++] = r;
		bufLED[offset++] = g;
		bufLED[offset++] = b;
	}
}


void sendLED() {
	ChangeBaud(9600);
	SendBuffer(bufLED, sizeof(bufLED));
}	


/* end of RGB-LED library */


short LEDupdate=1;
short r=0,g=0,b=0;
short pLED=0;


TestValSet(1);


GPSPositionData GPSData;


FlightBatteryStateData BatteryData;


FlightStatusData StatusData;


TestValSet(2);
sync(0);
while(TestValGet() > 0)
{


/* Battery */
	FlightBatteryStateGet(&BatteryData);
	if (BatteryData.Voltage < 10.4)
	{
		setLED(0,170,0,0);
		LEDupdate=1;
	}
	if ((BatteryData.Voltage < 10.8) && (BatteryData.Voltage > 10.4))
	{
		setLED(0,170,170,0);
		LEDupdate=1;
	}
	if (10.8 < BatteryData.Voltage)
	{
		setLED(0,0,170,0);
		LEDupdate=1;
	}
 
/* GPS */
	GPSPositionGet(&GPSData);
	if (GPSData.Satellites < 5) 
	{
		setLED(2,170,0,0);
		LEDupdate=1;
	}
	if ((GPSData.Satellites < 7) && (GPSData.Satellites > 4))
	{
		setLED(2,170,170,0);
		LEDupdate=1;
	}
	if ((6 < GPSData.Satellites) && (GPSData.PDOP <= 3.5))
	{
		setLED(2,0,170,0);
		LEDupdate=1;
	}
 
/* FlightMode */
	FlightStatusGet(&StatusData);
	/* Geofence */
	if (StatusData.ControlSource == 0)
	{


	}
	/* FailSafe */
	else if (StatusData.ControlSource == 1)  
	{
		//TMPtime = ((unsigned long)(time() / 100) % 2);
		if (pLED == 0) {
			pLED=1;
			setLED(1,0,170,0);
			LEDupdate=1;        			
		}
		else {
			pLED=0;
			setLED(1,170,0,0);
			LEDupdate=1;        			
		} 
	}
	/* Transmitter */	
	else if (StatusData.ControlSource == 2)
	{
		switch (StatusData.FlightMode) {
			case 1: //Acro
				setLED(1,170,0,0);
				LEDupdate=1;        			
				break;
			case 2: //Leveling
				setLED(1,0,170,0);
				LEDupdate=1;        			
				break;
			case 3: //VirtualBar
				setLED(1,0,170,170);
				LEDupdate=1;        			
				break;
			case 4: //Stabilized1
				setLED(1,170,170,0);
				LEDupdate=1;        			
				break;
			case 5: //Stabilized2
				setLED(1,170,170,0);
				LEDupdate=1;        			
				break;
			case 6: //Stabilized3
				setLED(1,170,170,0);
				LEDupdate=1;        			
				break;
			case 7: //Autotune
				setLED(1,170,170,170);
				LEDupdate=1;        			
				break;
			case 8: //AltitudeHold
				setLED(1,170,0,170);
				LEDupdate=1;        			
				break;
			case9: //VelocityControl
				setLED(1,75,0,130);
				LEDupdate=1;        			
				break;
			case 10: //PositionHold
				setLED(1,130,0,139);
				LEDupdate=1;        			
				break;
			case 11: //ReturnToHome
				setLED(1,255,0,0);
				LEDupdate=1;        			
				break;
		} 
		if (armed() || (pLED == 1)) {
			pLED = 0;
			LEDupdate=1;        			
		} else {
			pLED = 1;
			setLED(1,0,0,0);
			LEDupdate=1;        			
		}
	}
	/* Tablet */
	else if (StatusData.ControlSource == 3)  
	{


	}
 


// Set LED
	if (LEDupdate == 1) {
		sendLED();
		LEDupdate = 0;
	}


	sync(250);
}
TestValSet(99);