WebSVN - FlightCtrl - Diff - Rev 747 and 752 - /branches/V0.68d Code Redesign killagreg/GPS.c


#include <inttypes.h>
 
#include "fc.h"
#include "ubx.h"
#include "mymath.h"
#include "timer0.h"
#include "uart.h"
 
#define TSK_IDLE                0
#define TSK_HOLD                1
#define TSK_HOME                2
 
 
int16_t GPS_Pitch = 0;
int16_t GPS_Roll = 0;
 
int32_t GPS_P_Factor = 0, GPS_D_Factor = 0;
int32_t GPSPosDev_North = 0, GPSPosDev_East = 0;
 
 
typedef struct
{
        int32_t Northing;
        int32_t Easting;
        uint8_t Status;
 
} GPS_Pos_t;
 
// GPS coordinates for hold position
GPS_Pos_t HoldPosition  = {0,0, INVALID};
// GPS coordinates for home position
GPS_Pos_t HomePosition  = {0,0, INVALID};
 
 
// ---------------------------------------------------------------------------------
 
// set home position to current hold positon
void GPS_SetHomePosition(void)
{
                HomePosition.Northing = HoldPosition.Northing;
                HomePosition.Easting = HoldPosition.Easting;
                HomePosition.Status = HoldPosition.Status;
                if(HomePosition.Status == VALID) BeepTime = 1000; // signal if new home position was set
}
 
 
// disable GPS control sticks
void GPS_Neutral(void)
{
        GPS_Pitch = 0;
        GPS_Roll = 0;
}
 
// calculates the GPS control sticks values from the position deviation
void GPS_PDController(void)
{
        int32_t coscompass, sincompass;
        int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0;
        int32_t PD_North = 0, PD_East = 0;
        //Calculate PD-components of the controller (negative sign for compensation)
        P_North = -(GPS_P_Factor * GPSPosDev_North)/2048;
        D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512;
        P_East =  -(GPS_P_Factor * GPSPosDev_East)/2048;
        D_East =  -(GPS_D_Factor * GPSInfo.veleast)/512;
 
        // PD-controller
        PD_North = P_North + D_North;
        PD_East  = P_East  + D_East;
 
        // GPS to pitch and roll settings
 
        //A positive pitch angle moves head downwards (flying forward).
        //A positive roll angle tilts left side downwards (flying left).
 
        // If compass heading is 0 the head of the copter is in north direction.
        // A positive pitch angle will fly to north and a positive roll angle will fly to west.
        // In case of a positive north deviation/velocity the
        // copter should fly to south (negative pitch).
        // In case of a positive east position deviation and a positive east velocity the
        // copter should fly to west (positive roll).
 
        // The influence of the GPS_Pitch and GPS_Roll variable is contrarily to the stick values
        // in the fc.c. Therefore a positive north deviation/velocity should result in a positive
        // GPS_Pitch and a positive east deviation/velocity should result in a negative GPS_Roll.
 
        // rotation transformation to compass heading to match any copter orientation
        if (CompassHeading < 0) // no valid compass data
        { // disable GPS
                GPS_Neutral();
        }
        else
        {
                coscompass = (int32_t)c_cos_8192(CompassHeading);
                sincompass = (int32_t)c_sin_8192(CompassHeading);
                GPS_Roll  =    (int16_t)((coscompass * PD_East - sincompass * PD_North) / 8192);
                GPS_Pitch = -1*(int16_t)((sincompass * PD_East + coscompass * PD_North) / 8192);
        }
        // limit GPS controls
        #define GPS_CTRL_LIMIT 35
        if (GPS_Pitch >  GPS_CTRL_LIMIT) GPS_Pitch =  GPS_CTRL_LIMIT;
        if (GPS_Pitch < -GPS_CTRL_LIMIT) GPS_Pitch = -GPS_CTRL_LIMIT;
        if (GPS_Roll  >  GPS_CTRL_LIMIT)  GPS_Roll =  GPS_CTRL_LIMIT;
        if (GPS_Roll  < -GPS_CTRL_LIMIT)  GPS_Roll = -GPS_CTRL_LIMIT;
}
 
 
void GPS_Main(void)
{
        static uint8_t GPS_Task = TSK_IDLE;
 
        int16_t satbeep;
 
        // poti2 enables the gps feature
        if(Poti2 < 70) GPS_Task = TSK_IDLE;
        else if (Poti2 < 160) GPS_Task = TSK_HOLD;
        else GPS_Task = TSK_HOME; // Poti2 >= 160
 
 
        switch(GPSInfo.status)
        {
        case INVALID:  // invalid gps data
                GPS_Neutral();
                if(GPS_Task != TSK_IDLE)
                {
                        BeepTime = 100; // beep if signal is neccesary
                }
                break;
        case PROCESSED: // if gps data are already processed
                // downcount timeout
                if(GPSTimeout) GPSTimeout--;
                // if no new data arrived within timeout set current data invalid
                // and therefore disable GPS
                else
                {
                        GPS_Neutral();
                        GPSInfo.status = INVALID;
                }
                break;
        case VALID: // new valid data from gps device
                // if the gps data quality is sufficient
                if (GPSInfo.satfix == SATFIX_3D)
                {
                        switch(GPS_Task) // check what's to do
                        {
                                case TSK_IDLE:
                                        // update hold point to current gps position
                                        HoldPosition.Northing = GPSInfo.utmnorth;
                                        HoldPosition.Easting  = GPSInfo.utmeast;
                                        HoldPosition.Status = VALID;
                                        // disable gps control
                                        GPS_Neutral();
                                        break; // eof TSK_IDLE
                                case TSK_HOLD:
                                        // if sticks are centered and hold position is valid enable position hold control
                                        if ((MaxStickPitch < 20) && (MaxStickRoll < 20) && (HoldPosition.Status == VALID))
                                        {
                                                // Calculate deviation from hold position
                                                GPSPosDev_North = GPSInfo.utmnorth - HoldPosition.Northing;
                                                GPSPosDev_East  = GPSInfo.utmeast  - HoldPosition.Easting;
                                                DebugOut.Analog[12] = GPSPosDev_North;
                                                DebugOut.Analog[13] = GPSPosDev_East;
 
                                                GPS_PDController();
                                        }
                                        else // MK controlled by user
                                        {
                                                // update hold point to current gps position
                                                HoldPosition.Northing = GPSInfo.utmnorth;
                                                HoldPosition.Easting  = GPSInfo.utmeast;
                                                HoldPosition.Status = VALID;
                                                // disable gps control
                                                GPS_Neutral();
                                        }
                                        break; // eof TSK_HOLD
                                case TSK_HOME:
                                        if(HomePosition.Status == VALID)
                                        {
                                                // update hold point to current gps position
                                                HoldPosition.Northing = GPSInfo.utmnorth;
                                                HoldPosition.Easting  = GPSInfo.utmeast;
                                                HoldPosition.Status = VALID;
 
                                                // Calculate deviation from home position
                                                GPSPosDev_North = GPSInfo.utmnorth - HomePosition.Northing;
                                                GPSPosDev_East  = GPSInfo.utmeast  - HomePosition.Easting;
                                                DebugOut.Analog[12] = GPSPosDev_North;
                                                DebugOut.Analog[13] = GPSPosDev_East;
 
                                                GPS_PDController();
                                        }
                                        else // bad home position
                                        {
                                                GPS_Neutral();
                                                BeepTime = 50; // signal invalid home position
                                        }
                                        break; // eof TSK_HOME
                                default: // unhandled task
                                        GPS_Neutral();
                                        break; // eof default
                        } // eof switch GPS_Task
                } // eof 3D-FIX
 
                else // no 3D-SATFIX
                {       // disable gps control
                        GPS_Neutral();
                        if(GPS_Task != TSK_IDLE)
                        {
                                satbeep = 2048 - (int16_t)GPSInfo.satnum * 256; // is zero at 8 sats
                                if (satbeep < 0) satbeep = 0;
                                BeepTime = 50 + (uint16_t)satbeep;
                        }
                }
 
                // set current data as processed to avoid further calculations on the same gps data
                GPSInfo.status = PROCESSED;
                break;
        } // eof GPSInfo.status
        DebugOut.Analog[14] = GPS_Pitch;
        DebugOut.Analog[15] = GPS_Roll;
}
 
 

Rev 747	Rev 752
1	#include <inttypes.h>	1	#include <inttypes.h>
2		2
3	#include "fc.h"	3	#include "fc.h"
4	#include "ubx.h"	4	#include "ubx.h"
5	#include "mymath.h"	5	#include "mymath.h"
6	#include "timer0.h"	6	#include "timer0.h"
7	#include "uart.h"	7	#include "uart.h"
8		8
9	#define TSK_IDLE 0	9	#define TSK_IDLE 0
10	#define TSK_HOLD 1	10	#define TSK_HOLD 1
11	#define TSK_HOME 2	11	#define TSK_HOME 2
12		12
13		13
14	int16_t GPS_Pitch = 0;	14	int16_t GPS_Pitch = 0;
15	int16_t GPS_Roll = 0;	15	int16_t GPS_Roll = 0;
16		16
17	int32_t GPS_P_Factor = 0, GPS_D_Factor = 0;	17	int32_t GPS_P_Factor = 0, GPS_D_Factor = 0;
18	int32_t GPSPosDev_North = 0, GPSPosDev_East = 0;	18	int32_t GPSPosDev_North = 0, GPSPosDev_East = 0;
19		19
20		20
21	typedef struct	21	typedef struct
22	{	22	{
23	int32_t Northing;	23	int32_t Northing;
24	int32_t Easting;	24	int32_t Easting;
25	uint8_t Status;	25	uint8_t Status;
26		26
27	} GPS_Pos_t;	27	} GPS_Pos_t;
28		28
29	// GPS coordinates for hold position	29	// GPS coordinates for hold position
30	GPS_Pos_t HoldPosition = {0,0, INVALID};	30	GPS_Pos_t HoldPosition = {0,0, INVALID};
31	// GPS coordinates for home position	31	// GPS coordinates for home position
32	GPS_Pos_t HomePosition = {0,0, INVALID};	32	GPS_Pos_t HomePosition = {0,0, INVALID};
33		33
34		34
35	// ---------------------------------------------------------------------------------	35	// ---------------------------------------------------------------------------------
36		36
37	// set home position to current hold positon	37	// set home position to current hold positon
38	void GPS_SetHomePosition(void)	38	void GPS_SetHomePosition(void)
39	{	39	{
40	HomePosition.Northing = HoldPosition.Northing;	40	HomePosition.Northing = HoldPosition.Northing;
41	HomePosition.Easting = HoldPosition.Easting;	41	HomePosition.Easting = HoldPosition.Easting;
42	HomePosition.Status = HoldPosition.Status;	42	HomePosition.Status = HoldPosition.Status;
43	if(HomePosition.Status == VALID) BeepTime = 1000; // signal if new home position was set	43	if(HomePosition.Status == VALID) BeepTime = 1000; // signal if new home position was set
44	}	44	}
45		45
46		46
47	// disable GPS control sticks	47	// disable GPS control sticks
48	void GPS_Neutral(void)	48	void GPS_Neutral(void)
49	{	49	{
50	GPS_Pitch = 0;	50	GPS_Pitch = 0;
51	GPS_Roll = 0;	51	GPS_Roll = 0;
52	}	52	}
53		53
54	// calculates the GPS control sticks values from the position deviation	54	// calculates the GPS control sticks values from the position deviation
55	void GPS_PDController(void)	55	void GPS_PDController(void)
56	{	56	{
57	int32_t coscompass, sincompass;	57	int32_t coscompass, sincompass;
58	int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0;	58	int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0;
59	int32_t PD_North = 0, PD_East = 0;	59	int32_t PD_North = 0, PD_East = 0;
60	//Calculate PD-components of the controller (negative sign for compensation)	60	//Calculate PD-components of the controller (negative sign for compensation)
61	P_North = -(GPS_P_Factor * GPSPosDev_North)/2048;	61	P_North = -(GPS_P_Factor * GPSPosDev_North)/2048;
62	D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512;	62	D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512;
63	P_East = -(GPS_P_Factor * GPSPosDev_East)/2048;	63	P_East = -(GPS_P_Factor * GPSPosDev_East)/2048;
64	D_East = -(GPS_D_Factor * GPSInfo.veleast)/512;	64	D_East = -(GPS_D_Factor * GPSInfo.veleast)/512;
65		65
66	// PD-controller	66	// PD-controller
67	PD_North = P_North + D_North;	67	PD_North = P_North + D_North;
68	PD_East = P_East + D_East;	68	PD_East = P_East + D_East;
69		69
70	// GPS to pitch and roll settings	70	// GPS to pitch and roll settings
71		71
72	//A positive pitch angle moves head downwards (flying forward).	72	//A positive pitch angle moves head downwards (flying forward).
73	//A positive roll angle tilts left side downwards (flying left).	73	//A positive roll angle tilts left side downwards (flying left).
74		74
75	// If compass heading is 0 the head of the copter is in north direction.	75	// If compass heading is 0 the head of the copter is in north direction.
76	// A positive pitch angle will fly to north and a positive roll angle will fly to west.	76	// A positive pitch angle will fly to north and a positive roll angle will fly to west.
77	// In case of a positive north deviation/velocity the	77	// In case of a positive north deviation/velocity the
78	// copter should fly to south (negative pitch).	78	// copter should fly to south (negative pitch).
79	// In case of a positive east position deviation and a positive east velocity the	79	// In case of a positive east position deviation and a positive east velocity the
80	// copter should fly to west (positive roll).	80	// copter should fly to west (positive roll).
81		81
82	// The influence of the GPS_Pitch and GPS_Roll variable is contrarily to the stick values	82	// The influence of the GPS_Pitch and GPS_Roll variable is contrarily to the stick values
83	// in the fc.c. Therefore a positive north deviation/velocity should result in a positive	83	// in the fc.c. Therefore a positive north deviation/velocity should result in a positive
84	// GPS_Pitch and a positive east deviation/velocity should result in a negative GPS_Roll.	84	// GPS_Pitch and a positive east deviation/velocity should result in a negative GPS_Roll.
85		85
86	// rotation transformation to compass heading to match any copter orientation	86	// rotation transformation to compass heading to match any copter orientation
87	if (CompassHeading < 0) // no valid compass data	87	if (CompassHeading < 0) // no valid compass data
88	{ // disable GPS	88	{ // disable GPS
89	GPS_Neutral();	89	GPS_Neutral();
90	}	90	}
91	else	91	else
92	{	92	{
93	coscompass = (int32_t)c_cos_8192(CompassHeading);	93	coscompass = (int32_t)c_cos_8192(CompassHeading);
94	sincompass = (int32_t)c_sin_8192(CompassHeading);	94	sincompass = (int32_t)c_sin_8192(CompassHeading);
95	GPS_Roll = (int16_t)((coscompass * PD_East - sincompass * PD_North) / 8192);	95	GPS_Roll = (int16_t)((coscompass * PD_East - sincompass * PD_North) / 8192);
96	GPS_Pitch = -1(int16_t)((sincompass PD_East + coscompass * PD_North) / 8192);	96	GPS_Pitch = -1(int16_t)((sincompass PD_East + coscompass * PD_North) / 8192);
97	}	97	}
98	// limit GPS controls	98	// limit GPS controls
99	#define GPS_CTRL_LIMIT 35	99	#define GPS_CTRL_LIMIT 35
100	if (GPS_Pitch > GPS_CTRL_LIMIT) GPS_Pitch = GPS_CTRL_LIMIT;	100	if (GPS_Pitch > GPS_CTRL_LIMIT) GPS_Pitch = GPS_CTRL_LIMIT;
101	if (GPS_Pitch < -GPS_CTRL_LIMIT) GPS_Pitch = -GPS_CTRL_LIMIT;	101	if (GPS_Pitch < -GPS_CTRL_LIMIT) GPS_Pitch = -GPS_CTRL_LIMIT;
102	if (GPS_Roll > GPS_CTRL_LIMIT) GPS_Roll = GPS_CTRL_LIMIT;	102	if (GPS_Roll > GPS_CTRL_LIMIT) GPS_Roll = GPS_CTRL_LIMIT;
103	if (GPS_Roll < -GPS_CTRL_LIMIT) GPS_Roll = -GPS_CTRL_LIMIT;	103	if (GPS_Roll < -GPS_CTRL_LIMIT) GPS_Roll = -GPS_CTRL_LIMIT;
104	}	104	}
105		105
106		106
107	void GPS_Main(void)	107	void GPS_Main(void)
108	{	108	{
109	static uint8_t GPS_Task = TSK_IDLE;	109	static uint8_t GPS_Task = TSK_IDLE;
110		-
-		110	int16_t satbeep;
111		111
112	// poti2 enables the gps feature	112	// poti2 enables the gps feature
113	if(Poti2 < 70) GPS_Task = TSK_IDLE;	113	if(Poti2 < 70) GPS_Task = TSK_IDLE;
114	else if (Poti2 < 160) GPS_Task = TSK_HOLD;	114	else if (Poti2 < 160) GPS_Task = TSK_HOLD;
115	else GPS_Task = TSK_HOME; // Poti2 >= 160	115	else GPS_Task = TSK_HOME; // Poti2 >= 160
116		116
117		117
118	switch(GPSInfo.status)	118	switch(GPSInfo.status)
119	{	119	{
120	case INVALID: // invalid gps data	120	case INVALID: // invalid gps data
121	GPS_Neutral();	121	GPS_Neutral();
-		122	if(GPS_Task != TSK_IDLE)
-		123	{
122	if(GPS_Task != TSK_IDLE) BeepTime = 50; // beep if signal is neccesary	124	BeepTime = 100; // beep if signal is neccesary
-		125	}
123	break;	126	break;
124	case PROCESSED: // if gps data are already processed	127	case PROCESSED: // if gps data are already processed
125	// downcount timeout	128	// downcount timeout
126	if(GPSTimeout) GPSTimeout--;	129	if(GPSTimeout) GPSTimeout--;
127	// if no new data arrived within timeout set current data invalid	130	// if no new data arrived within timeout set current data invalid
128	// and therefore disable GPS	131	// and therefore disable GPS
129	else	132	else
130	{	133	{
131	GPS_Neutral();	134	GPS_Neutral();
132	GPSInfo.status = INVALID;	135	GPSInfo.status = INVALID;
133	}	136	}
134	break;	137	break;
135	case VALID: // new valid data from gps device	138	case VALID: // new valid data from gps device
136	// if the gps data quality is sufficient	139	// if the gps data quality is sufficient
137	if (GPSInfo.satfix == SATFIX_3D)	140	if (GPSInfo.satfix == SATFIX_3D)
138	{	141	{
139	switch(GPS_Task) // check what's to do	142	switch(GPS_Task) // check what's to do
140	{	143	{
141	case TSK_IDLE:	144	case TSK_IDLE:
142	// update hold point to current gps position	145	// update hold point to current gps position
143	HoldPosition.Northing = GPSInfo.utmnorth;	146	HoldPosition.Northing = GPSInfo.utmnorth;
144	HoldPosition.Easting = GPSInfo.utmeast;	147	HoldPosition.Easting = GPSInfo.utmeast;
145	HoldPosition.Status = VALID;	148	HoldPosition.Status = VALID;
146	// disable gps control	149	// disable gps control
147	GPS_Neutral();	150	GPS_Neutral();
148	break; // eof TSK_IDLE	151	break; // eof TSK_IDLE
149	case TSK_HOLD:	152	case TSK_HOLD:
150	// if sticks are centered and hold position is valid enable position hold control	153	// if sticks are centered and hold position is valid enable position hold control
151	if ((MaxStickPitch < 20) && (MaxStickRoll < 20) && (HoldPosition.Status == VALID))	154	if ((MaxStickPitch < 20) && (MaxStickRoll < 20) && (HoldPosition.Status == VALID))
152	{	155	{
153	// Calculate deviation from hold position	156	// Calculate deviation from hold position
154	GPSPosDev_North = GPSInfo.utmnorth - HoldPosition.Northing;	157	GPSPosDev_North = GPSInfo.utmnorth - HoldPosition.Northing;
155	GPSPosDev_East = GPSInfo.utmeast - HoldPosition.Easting;	158	GPSPosDev_East = GPSInfo.utmeast - HoldPosition.Easting;
156	DebugOut.Analog[12] = GPSPosDev_North;	159	DebugOut.Analog[12] = GPSPosDev_North;
157	DebugOut.Analog[13] = GPSPosDev_East;	160	DebugOut.Analog[13] = GPSPosDev_East;
158		161
159	GPS_PDController();	162	GPS_PDController();
160	}	163	}
161	else // MK controlled by user	164	else // MK controlled by user
162	{	165	{
163	// update hold point to current gps position	166	// update hold point to current gps position
164	HoldPosition.Northing = GPSInfo.utmnorth;	167	HoldPosition.Northing = GPSInfo.utmnorth;
165	HoldPosition.Easting = GPSInfo.utmeast;	168	HoldPosition.Easting = GPSInfo.utmeast;
166	HoldPosition.Status = VALID;	169	HoldPosition.Status = VALID;
167	// disable gps control	170	// disable gps control
168	GPS_Neutral();	171	GPS_Neutral();
169	}	172	}
170	break; // eof TSK_HOLD	173	break; // eof TSK_HOLD
171	case TSK_HOME:	174	case TSK_HOME:
172	if(HomePosition.Status == VALID)	175	if(HomePosition.Status == VALID)
173	{	176	{
174	// update hold point to current gps position	177	// update hold point to current gps position
175	HoldPosition.Northing = GPSInfo.utmnorth;	178	HoldPosition.Northing = GPSInfo.utmnorth;
176	HoldPosition.Easting = GPSInfo.utmeast;	179	HoldPosition.Easting = GPSInfo.utmeast;
177	HoldPosition.Status = VALID;	180	HoldPosition.Status = VALID;
178		181
179	// Calculate deviation from home position	182	// Calculate deviation from home position
180	GPSPosDev_North = GPSInfo.utmnorth - HomePosition.Northing;	183	GPSPosDev_North = GPSInfo.utmnorth - HomePosition.Northing;
181	GPSPosDev_East = GPSInfo.utmeast - HomePosition.Easting;	184	GPSPosDev_East = GPSInfo.utmeast - HomePosition.Easting;
182	DebugOut.Analog[12] = GPSPosDev_North;	185	DebugOut.Analog[12] = GPSPosDev_North;
183	DebugOut.Analog[13] = GPSPosDev_East;	186	DebugOut.Analog[13] = GPSPosDev_East;
184		187
185	GPS_PDController();	188	GPS_PDController();
186	}	189	}
187	else // bad home position	190	else // bad home position
188	{	191	{
189	GPS_Neutral();	192	GPS_Neutral();
190	BeepTime = 50; // signal invalid home position	193	BeepTime = 50; // signal invalid home position
191	}	194	}
192	break; // eof TSK_HOME	195	break; // eof TSK_HOME
193	default: // unhandled task	196	default: // unhandled task
194	GPS_Neutral();	197	GPS_Neutral();
195	break; // eof default	198	break; // eof default
196	} // eof switch GPS_Task	199	} // eof switch GPS_Task
197	} // eof 3D-FIX	200	} // eof 3D-FIX
198		201
199	else // no 3D-SATFIX	202	else // no 3D-SATFIX
200	{ // disable gps control	203	{ // disable gps control
201	GPS_Neutral();	204	GPS_Neutral();
202	if(GPS_Task != TSK_IDLE) BeepTime = 50;	205	if(GPS_Task != TSK_IDLE)
-		206	{
-		207	satbeep = 2048 - (int16_t)GPSInfo.satnum * 256; // is zero at 8 sats
-		208	if (satbeep < 0) satbeep = 0;
-		209	BeepTime = 50 + (uint16_t)satbeep;
-		210	}
203	}	211	}
204		-
205	// set current data as processed to avoid further calculations on the same gps data	212	// set current data as processed to avoid further calculations on the same gps data
206	GPSInfo.status = PROCESSED;	213	GPSInfo.status = PROCESSED;
207	break;	214	break;
208	} // eof GPSInfo.status	215	} // eof GPSInfo.status
209	DebugOut.Analog[14] = GPS_Pitch;	216	DebugOut.Analog[14] = GPS_Pitch;
210	DebugOut.Analog[15] = GPS_Roll;	217	DebugOut.Analog[15] = GPS_Roll;
211	}	218	}
212		219
213		220

Subversion Repositories FlightCtrl

(root)/branches/V0.68d Code Redesign killagreg/GPS.c - Rev 747 → 752