/*
 * SBUS.c
 *
 * Created: 26.05.2016 12:36:35
 *  Author: Philipp
 */ 

#include <avr/io.h>
#include <avr/interrupt.h>
#include <stdlib.h>
#include "uart.h"
#include "SBUS.h"

	uint16_t	SBUS_Channel[16];	// 16 Proportional Kanäle
	uint8_t		SBUS_Byte[24];		// 25 Datenframes
	uint8_t		SBUS_Byteindex;
	uint8_t		SBUS_lost_Frames;


	
ISR (USART_RX_vect)		{
	uint8_t byte = UDR0;
	
	// Dieser Teil gehört in die Hauptschleife
	
	// Prüfe Synchronisierung
	if(SBUS_Flags.SBUS_synced){	
		//Lese Frame
		if (SBUS_Byteindex<24){
			SBUS_Byte[SBUS_Byteindex++]=byte;
		}
		else if (byte == SBUS_Endbyte)			{	
			SBUS_Flags.New_Frame_received=1;
			SBUS_Byteindex=0;
		}
		else {
			SBUS_Flags.SBUS_synced=0;// Synchronisierung verloren
			SBUS_lost_Frames++;
		}
	}
	else { 
		// Synchronisiere
		if(byte==SBUS_Endbyte){
			SBUS_Flags.Endbyte_received=1;
		}
		else if (byte==SBUS_Startbyte && SBUS_Flags.Endbyte_received){
			SBUS_Flags.SBUS_synced=1;
			SBUS_Flags.Endbyte_received=0;
			SBUS_Byteindex=1;
			SBUS_Byte[0]=SBUS_Startbyte;
		}		
	}
}

void SBUS_Frame_to_Channels(){
	if (SBUS_Byte[0]!=SBUS_Startbyte ){
		SBUS_Flags.Frame_valid=0;
		return;
	}
	else{
		SBUS_Channel[0]	 = ((SBUS_Byte[1]	 | (8<<SBUS_Byte[2])) & 0x7FF );
		SBUS_Channel[1]  = ((SBUS_Byte[2]>>3 |SBUS_Byte[3]<<5)                 & 0x07FF);
		SBUS_Channel[2]  = ((SBUS_Byte[3]>>6 |SBUS_Byte[4]<<2 |SBUS_Byte[5]<<10)  & 0x07FF);
		SBUS_Channel[3]  = ((SBUS_Byte[5]>>1 |SBUS_Byte[6]<<7)                 & 0x07FF);
		SBUS_Channel[4]  = ((SBUS_Byte[6]>>4 |SBUS_Byte[7]<<4)                 & 0x07FF);
		SBUS_Channel[5]  = ((SBUS_Byte[7]>>7 |SBUS_Byte[8]<<1 |SBUS_Byte[9]<<9)   & 0x07FF);
		SBUS_Channel[6]  = ((SBUS_Byte[9]>>2 |SBUS_Byte[10]<<6)                & 0x07FF);
		SBUS_Channel[7]  = ((SBUS_Byte[10]>>5|SBUS_Byte[11]<<3)                & 0x07FF);
		SBUS_Channel[8]  = ((SBUS_Byte[12]   |SBUS_Byte[13]<<8)                & 0x07FF);
		SBUS_Channel[9]  = ((SBUS_Byte[13]>>3|SBUS_Byte[14]<<5)                & 0x07FF);
		SBUS_Channel[10] = ((SBUS_Byte[14]>>6|SBUS_Byte[15]<<2|SBUS_Byte[16]<<10) & 0x07FF);
		SBUS_Channel[11] = ((SBUS_Byte[16]>>1|SBUS_Byte[17]<<7)                & 0x07FF);
		SBUS_Channel[12] = ((SBUS_Byte[17]>>4|SBUS_Byte[18]<<4)                & 0x07FF);
		SBUS_Channel[13] = ((SBUS_Byte[18]>>7|SBUS_Byte[19]<<1|SBUS_Byte[20]<<9)  & 0x07FF);
		SBUS_Channel[14] = ((SBUS_Byte[20]>>2|SBUS_Byte[21]<<6)                & 0x07FF);
		SBUS_Channel[15] = ((SBUS_Byte[21]>>5|SBUS_Byte[22]<<3)                & 0x07FF);
		SBUS_Flags.SBUS_Channel_16 = (SBUS_Byte[23] & 0b1000000 );		// Bit 7
		SBUS_Flags.SBUS_Channel_17 = (SBUS_Byte[23] & 0b01000000 );		// Bit 6
		SBUS_lost_Frames = SBUS_lost_Frames + (SBUS_Byte[23] & 0b00100000 );		// Funktioniert so nicht!!!
		SBUS_Flags.Failsave_activated = (SBUS_Byte[23] & 0b00010000 );
		SBUS_Flags.Frame_valid = 1;
	}
}

void send_Channels_over_UART(){
		  // Sende Kanäle über UART
	  	  char buffer[5];
		  for(uint8_t i=0; i<16;i++){
			  uart_puts("Kanal ");
			  uart_putc(i);
			  uart_puts(":  ");
			  utoa(SBUS_Channel[i],buffer, 10);
			  uart_putc('\n');
		  }
		  
}