Commits

bsaid committed b2a8497

Optimized for driving Junior flying wing.

Comments (0)

Files changed (6)

firmware/YuniflyLibraries/accelerometer.hpp

 //#include "uart.hpp"
 
 int32_t accData[8];	//saved accelerometers data
-int32_t sumAcc[8];	//integration of accData for detecting absolute position
+int32_t computeAcc[8];	//integration of accData for detecting absolute position
 int32_t staticErrorsAcc[8];	//detected deviation after first calibration
 
 void accInit()
 {
 	bool captured = false;
 	static int point = 0;
-	char aa = rs2321.get();
+	char aa;
+	if(!rs2321.peek(aa))
+		return true;
 	if(aa == 's' && point == 0)
 	{
 		point++;
 							accData[i+2] -= 65535;
 					}
 					captured = true;
-					for(int i=2;i<8;i++)		//delete this cycle, only for help
-					{
-						sumAcc[i] += accData[i] - staticErrorsAcc[i];
-					}
 				}
 			}
 			point = 0;
 }
 
 
-void firstAccCalibration(int cycles = 1000)	//this function takes long time (a few seconds)
+/*void firstAccCalibration(int cycles = 1000)	//this function takes long time (a few seconds)
 {
 	for(int i=2;i<8;i++)
 	{
 	{
 		staticErrorsAcc[i] = sumAcc[i] / cycles;
 	}
-}
+}*/

firmware/YuniflyLibraries/fastPWM.hpp

 //#include "basic.hpp"
 
-int const minPWMvalue = -32;	//min is -127, set -32 for debug
-int const maxPWMvalue = 32;		//max is 127, set 32 for debug
+int const minPWMvalue = -64;	//min is -127, set -32 for debug
+int const maxPWMvalue = 64;		//max is 127, set 32 for debug
+
+int omezeni(int value)
+{
+	if(value > maxPWMvalue)
+		value = maxPWMvalue;
+	else if(value < minPWMvalue)
+		value = minPWMvalue;
+	return value;
+}
 
 int convertPWMValue(int value)
 {
 
 void setFastPWM(int id, int value)	//value -127; +127
 {
+	value = omezeni(value);
 	if(id == 0)
 		OCR1A = convertPWMValue(value);
 	else if(id == 1)
 
 void setAllPWMs(int v1, int v2, int v3, int v4, int v5, int v6)
 {
+	v1 = omezeni(v1);
+	v2 = omezeni(v2);
+	v3 = omezeni(v3);
+	v4 = omezeni(v4);
+	v5 = omezeni(v5);
+	v6 = omezeni(v6);
 	OCR1A = convertPWMValue(v1);
 	OCR1B = convertPWMValue(v2);
 	OCR1C = convertPWMValue(v3);

firmware/YuniflyLibraries/firmware.aws

-<AVRWorkspace><IOSettings><CurrentRegisters><USART1><register register="UBRR1H" group="USART1" display="1" locked="0"/></USART1><USART1><register register="UBRR1L" group="USART1" display="1" locked="0"/></USART1><USART1><register register="UCSR1A" group="USART1" display="1" locked="0"/></USART1><USART1><register register="UCSR1B" group="USART1" display="1" locked="0"/></USART1><USART1><register register="UCSR1C" group="USART1" display="1" locked="0"/></USART1><USART1><register register="UDR1" group="USART1" display="1" locked="0"/></USART1></CurrentRegisters></IOSettings><part name="ATMEGA128"/><Files><File00000 Name="D:\Beda\Aktualni data\Yunifly\Yunifly\YuniflyLibraries\firmware.c" Position="909 769 1069 796" LineCol="43 0"/><File00001 Name="D:\Beda\Aktualni data\Yunifly\Yunifly\YuniflyLibraries\uart.hpp" Position="749 769 909 796" LineCol="6 0"/><File00002 Name="D:\Beda\Aktualni data\Yunifly\Yunifly\YuniflyLibraries\accelerometer.hpp" Position="83 120 856 701" LineCol="36 65"/><File00003 Name="D:\Beda\Aktualni data\Yunifly\Yunifly\YuniflyLibraries\paralTransmitter.hpp" Position="429 769 589 796" LineCol="9 0"/><File00004 Name="D:\Beda\Aktualni data\Yunifly\Yunifly\YuniflyLibraries\fastPWM.hpp" Position="269 769 429 796" LineCol="62 0"/><File00005 Name="D:\Beda\Aktualni data\Yunifly\Yunifly\YuniflyLibraries\led.hpp" Position="109 769 269 796" LineCol="3 0"/></Files></AVRWorkspace>
+<AVRWorkspace><IOSettings><CurrentRegisters/></IOSettings><part name="ATMEGA128"/><Files><File00000 Name="D:\Beda\Aktualni data\Yunifly\Yunifly\YuniflyLibraries\firmware.c" Position="178 443 1274 741" LineCol="48 59"/><File00001 Name="D:\Beda\Aktualni data\Yunifly\Yunifly\YuniflyLibraries\uart.hpp" Position="126 124 1214 392" LineCol="6 0"/><File00002 Name="D:\Beda\Aktualni data\Yunifly\Yunifly\YuniflyLibraries\accelerometer.hpp" Position="261 761 421 788" LineCol="76 3"/><File00003 Name="D:\Beda\Aktualni data\Yunifly\Yunifly\YuniflyLibraries\paralTransmitter.hpp" Position="176 174 1264 442" LineCol="6 74"/><File00004 Name="D:\Beda\Aktualni data\Yunifly\Yunifly\YuniflyLibraries\fastPWM.hpp" Position="421 761 581 788" LineCol="62 3"/><File00005 Name="D:\Beda\Aktualni data\Yunifly\Yunifly\YuniflyLibraries\led.hpp" Position="226 224 1314 492" LineCol="48 0"/></Files></AVRWorkspace>

firmware/YuniflyLibraries/firmware.c

 	blink();
 	//firstAccCalibration();
 	//blink();
-	testRangePWM();
-	blink();
+	//testRangePWM();
+	//blink();
+
 	//BTRCinit();	//todo: uncomment or delete with declaration; set concurent access
 	//accInit();	//todo: uncomment or delete with declaration; set concurent access
+	int accPropConst = 200;
 	
-	int plus = 1;
 	for(;;)
 	{
-		BTRCupdate();
-
-		//driving by accelerometers
-		for(int i=0;i<channels;i++)
+		//set constant for proporcional accelerometer driving
+		if(buttons[1])
 		{
-			wantAcc[i] = RCchannels[i]/8;
+			accPropConst = RCchannels[3];
+		}
+		
+		for(int i=0;i<5;i++)	//todo: set asynchronic cycle for reading faster accelerometer uart line
+		{
+			while(!BTRCupdate());
 		}
 
-		for(int i=0;i<channels;i++)
-		{
-			setFastPWM( servosPosition[i], wantAcc[i]);
-		}
+		//driving by accelerometers and transmitter (in user option)
+		wantAcc[2] = RCchannels[2]/8;	//motor
+		wantAcc[0] = -RCchannels[1]/8 + RCchannels[0]/8;	//servo0
+		wantAcc[1] = -RCchannels[1]/8 - RCchannels[0]/8;	//servo1
+		computeAcc[2] = 0;	//motor is not driven by accelerometer
+		computeAcc[0] = -accData[3]*accReverzCorrection[3]/accPropConst + accData[4]*accReverzCorrection[4]/accPropConst;	//accelerometer stabilization for servo0
+		computeAcc[1] = -accData[3]*accReverzCorrection[3]/accPropConst - accData[4]*accReverzCorrection[4]/accPropConst;	//accelerometer stabilization for servo1
 
-		/*for(int i=0;i<20;i++)	//todo: set asynchronic cycle for reading faster accelerometer uart line
+		for(int i=0;i<5;i++)	//todo: set asynchronic cycle for reading faster accelerometer uart line
 		{
 			while(!accUpdate());
 		}
 
 		for(int i=0;i<channels;i++)
 		{
-			if(drivenByAcc[i] && getBit(buttons,0))
+			if(drivenByAcc[i] && buttons[0])
 			{
-				setFastPWM( servosPosition[i], (accData[ accChannelsPosition[i] ]/100) * accReverzCorrection[i] - wantAcc[i]);
+				setFastPWM( servosPosition[i], wantAcc[i] - computeAcc[i] );
 			}
 			else
 			{
-				setFastPWM( servosPosition[i], wantAcc[i]);
+				setFastPWM( servosPosition[i], wantAcc[i] );
 			}
-		}*/
-		toggleLed();
+		}
 
+		if(buttons[0])
+			clearLed();
+		else
+			setLed();
+
+		//toggleLed();
 		/*debug.wait();
 		for(int j=2;j<8;j++)
 			debug << sumAcc[j] << "     |     ";

firmware/YuniflyLibraries/led.hpp

 //todo: need debug
 //#include "basic.hpp"
 
+
 void initLEDs()
 {
 	DDRB = 0xF7;
 	PORTE = 0xFF;
 }
 
+
+void setLed()
+{
+	PORTD |= (1<<5);
+	PORTE |= (1<<6);
+}
+
+
+void clearLed()
+{
+	PORTD &= ~(1<<5);
+	PORTE &= ~(1<<6);
+}
+
+
 void blink(int waitTime_ms = 500)
 {
 	//PORTD |= (1<<5);
 	//PORTD |= (1<<7);
-	PORTD |= (1<<5);
-	PORTE |= (1<<6);
+	setLed();
 	_delay_ms(waitTime_ms);
-	PORTD &= ~(1<<5);
-	PORTE &= ~(1<<6);
+	clearLed();
 	_delay_ms(waitTime_ms);
-	PORTD |= (1<<5);
-	PORTE |= (1<<6);
+	setLed();
 }
 
+
 void toggleLed()
 {
 	static int led = 0;
 	if(led % 10 == 0)
 	{
-		PORTD &= ~(1<<5);
-		PORTE &= ~(1<<6);
+		clearLed();
 	}
 	else if(led % 10 == 2)
 	{
-		PORTD |= (1<<5);
-		PORTE |= (1<<6);
+		setLed();
 	}
 	led++;
 }

firmware/YuniflyLibraries/paralTransmitter.hpp

 int const startByte = 0xFF;
 
 int const servosPosition[channels] = {0,1,2,3,4,5};		//configuration of PWMs and transmitter channels
-bool const drivenByAcc[channels] = {true, true, true, true, true};		//configuration of channels driven by accelerometers stabilization
+bool const drivenByAcc[channels] = {true, true, false, false, false, false};		//configuration of channels driven by accelerometers stabilization
 int const accChannelsPosition[channels] = {2,3,4,5,6,7};					//configuration of accelerotmeter channels and transmitter channels
 
 int accReverzCorrection[channels] = {1,1,1,1,1,1};					//set reverz and multiplied correction for transmitter channels
 int32_t RCchannels[channels];
 int32_t wantAcc[channels];
-char buttons = 0;
+bool buttons[2];
 
 bool getBit(char value, int poce)
 {
 }
 
 
-void BTRCupdate()
+bool BTRCupdate()
 {
 	//parser for Paral's transmitter
 	static int error = 0;
-	if(rs232.get() == startByte)
+	char firstCaptured;
+	if(!rs232.peek(firstCaptured))
+		return true;
+	if(firstCaptured == startByte)
 	{
 		int addr = rs232.get();	//adresa
 		int len = rs232.get();	//delka v bytech
 		int cmd = rs232.get();	//prikaz
-		for(int i=0;i<(len-1)/2;i++)	//melo by byt i<payloadSize-1, ted neresim; -1 kvuli hodnote prepinacu, ktere taky neresim
+
+		if(cmd == 0)
 		{
-			RCchannels[i] = rs232.get()*256 + rs232.get();
-			if(RCchannels[i] > 32768)
-				RCchannels[i] -= 65535;
+			for(int i=0;i<(len-1)/2;i++)	//kazdy kanal - 16bit, potom jeden byte na prikaz, proto (len-1)/2
+			{
+				RCchannels[i] = rs232.get()*256 + rs232.get();
+				if(RCchannels[i] > 32768)
+					RCchannels[i] -= 65535;
+			}
 		}
+		if(cmd == 1)
+		{
+			if(rs232.get() == 1)
+				buttons[0] = false;
+			else
+				buttons[0] = true;
+			if(rs232.get() == 1)
+				buttons[1] = false;
+			else
+				buttons[1] = true;
+		}
+		return true;
 	}
 	else
 	{
 		error++;
-	}
-	if(error == 100)
-	{
-		for(int i=0;i<channels;i++)
+		if(error == 100)
 		{
-			RCchannels[channels] = 0;
+			for(int i=0;i<channels;i++)
+			{
+				RCchannels[i] = 0;
+			}
+			error = 0;
 		}
-		error = 0;
-		blink();
+		return false;
 	}
 }