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


#include <inttypes.h>
#include <stdlib.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
 
#define GPS_STICK_SENSE         20              // must be at least in a range where 90% of the trimming does not switch of the GPS function
#define GPS_STICK_LIMIT         45              // limit of gps stick control to avoid critical flight attitudes
#define GPS_D_LIMIT_DIST        500     // for position deviations grater than 500 cm (20m) the D-Part of the PD-Controller is limited
#define GPS_D_LIMIT                     50              // PD-Controntroller D-Limit.
 
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
}
 
// clear home position
void GPS_ClearHomePosition(void)
{
                HomePosition.Status = INVALID;
}
 
// disable GPS control sticks
void GPS_Neutral(void)
{
        GPS_Pitch = 0;
        GPS_Roll = 0;
}
 
// calculates the GPS control sticks values from the deviation to target position
void GPS_PDController(GPS_Pos_t *TargetPos)
{
        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;
 
        if( (TargetPos->Status == VALID) && (GPSInfo.status == VALID) && (GPSInfo.satfix == SATFIX_3D))
        {
                GPSPosDev_North = GPSInfo.utmnorth - TargetPos->Northing;
                GPSPosDev_East  = GPSInfo.utmeast  - TargetPos->Easting;
 
                DebugOut.Analog[12] = GPSPosDev_North;
                DebugOut.Analog[13] = GPSPosDev_East;
 
                //DebugOut.Analog[12] = GPSInfo.velnorth;
                //DebugOut.Analog[13] = GPSInfo.veleast;
 
                //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;
 
                if( (abs(GPSPosDev_North) > GPS_D_LIMIT_DIST) ||  (abs(GPSPosDev_East) > GPS_D_LIMIT_DIST) )
                {
                        if (D_North > GPS_D_LIMIT) D_North = GPS_D_LIMIT;
                        else if (D_North < -GPS_D_LIMIT) D_North = -GPS_D_LIMIT;
                        if (D_East > GPS_D_LIMIT)  D_East  = GPS_D_LIMIT;
                        else if (D_East  < -GPS_D_LIMIT) D_East  = -GPS_D_LIMIT;
                }
 
                // 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
                if (GPS_Pitch >  GPS_STICK_LIMIT) GPS_Pitch =  GPS_STICK_LIMIT;
                else if (GPS_Pitch < -GPS_STICK_LIMIT) GPS_Pitch = -GPS_STICK_LIMIT;
                if (GPS_Roll  >  GPS_STICK_LIMIT)  GPS_Roll =  GPS_STICK_LIMIT;
                else if (GPS_Roll  < -GPS_STICK_LIMIT)  GPS_Roll = -GPS_STICK_LIMIT;
        }
        else // invalid input data
        {
                BeepTime = 50;
                GPS_Neutral();
        }
}
 
 
void GPS_Main(uint8_t ctrl)
{
        static uint8_t GPS_Task = TSK_IDLE;
        int16_t satbeep;
 
        // ctrl enables the gps feature
        if(ctrl < 70) GPS_Task = TSK_IDLE;
        else if (ctrl < 160) GPS_Task = TSK_HOLD;
        else GPS_Task = TSK_HOME; // ctrl >= 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(HoldPosition.Status == VALID)
                                        {
                                                // if sticks are centered (no manual control)
                                                if ((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE))
                                                {
                                                        GPS_PDController(&HoldPosition);
                                                }
                                                else // MK controlled by user
                                                {
                                                        // update hold point to current gps position
                                                        HoldPosition.Northing = GPSInfo.utmnorth;
                                                        HoldPosition.Easting  = GPSInfo.utmeast;
                                                        HoldPosition.Status   = GPSInfo.status;
                                                        // disable gps control
                                                        GPS_Neutral();
                                                }
                                        }
                                        break; // eof TSK_HOLD
                                case TSK_HOME:
                                        if(HomePosition.Status == VALID)
                                        {
                                                // update hold point to current gps position
                                                // to avoid a flight back if home comming is deactivated
                                                HoldPosition.Northing = GPSInfo.utmnorth;
                                                HoldPosition.Easting  = GPSInfo.utmeast;
                                                HoldPosition.Status   = GPSInfo.status;
                                                // if sticks are centered (no manual control)
                                                if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE))
                                                {
                                                        GPS_PDController(&HomePosition);
                                                }
                                                else // manual control
                                                {
                                                        GPS_Neutral();
                                                }
                                        }
                                        else // bad home position
                                        {
                                                BeepTime = 50; // signal invalid home position
                                                // try to hold at least the position as a fallback option
                                                // if sticks are centered (no manual control)
                                                if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE) && (HoldPosition.Status == VALID))
                                                {
                                                        GPS_PDController(&HoldPosition);
                                                }
                                                else // manual control or no rteference position
                                                {
                                                        GPS_Neutral();
                                                }
                                        }
                                        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 = 1800 - (int16_t)GPSInfo.satnum * 225; // is zero at 8 sats
                                if (satbeep < 0) satbeep = 0;
                                BeepTime = 50 + (uint16_t)satbeep; // max 1850 * 0.1 ms =
                        }
                }
                // 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 767	Rev 779
1	#include <inttypes.h>	1	#include <inttypes.h>
2	#include <stdlib.h>	2	#include <stdlib.h>
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	#define GPS_STICK_SENSE 20 // must be at least in a range where 90% of the trimming does not switch of the GPS function	13	#define GPS_STICK_SENSE 20 // must be at least in a range where 90% of the trimming does not switch of the GPS function
14	#define GPS_STICK_LIMIT 45 // limit of gps stick control to avoid critical flight attitudes	14	#define GPS_STICK_LIMIT 45 // limit of gps stick control to avoid critical flight attitudes
15	#define GPS_D_LIMIT_DIST 2000 // for position deviations grater than 500 cm (20m) the D-Part of the PD-Controller is limited	15	#define GPS_D_LIMIT_DIST 500 // for position deviations grater than 500 cm (20m) the D-Part of the PD-Controller is limited
16	#define GPS_D_LIMIT 50 // PD-Controntroller D-Limit.	16	#define GPS_D_LIMIT 50 // PD-Controntroller D-Limit.
17		17
18	int16_t GPS_Pitch = 0;	18	int16_t GPS_Pitch = 0;
19	int16_t GPS_Roll = 0;	19	int16_t GPS_Roll = 0;
20		20
21	int32_t GPS_P_Factor = 0, GPS_D_Factor = 0;	21	int32_t GPS_P_Factor = 0, GPS_D_Factor = 0;
22	int32_t GPSPosDev_North = 0, GPSPosDev_East = 0;	22	int32_t GPSPosDev_North = 0, GPSPosDev_East = 0;
23		23
24		24
25	typedef struct	25	typedef struct
26	{	26	{
27	int32_t Northing;	27	int32_t Northing;
28	int32_t Easting;	28	int32_t Easting;
29	uint8_t Status;	29	uint8_t Status;
30		30
31	} GPS_Pos_t;	31	} GPS_Pos_t;
32		32
33	// GPS coordinates for hold position	33	// GPS coordinates for hold position
34	GPS_Pos_t HoldPosition = {0,0, INVALID};	34	GPS_Pos_t HoldPosition = {0,0, INVALID};
35	// GPS coordinates for home position	35	// GPS coordinates for home position
36	GPS_Pos_t HomePosition = {0,0, INVALID};	36	GPS_Pos_t HomePosition = {0,0, INVALID};
37		37
38		38
39	// ---------------------------------------------------------------------------------	39	// ---------------------------------------------------------------------------------
40		40
41	// set home position to current hold positon	41	// set home position to current hold positon
42	void GPS_SetHomePosition(void)	42	void GPS_SetHomePosition(void)
43	{	43	{
44	HomePosition.Northing = HoldPosition.Northing;	44	HomePosition.Northing = HoldPosition.Northing;
45	HomePosition.Easting = HoldPosition.Easting;	45	HomePosition.Easting = HoldPosition.Easting;
46	HomePosition.Status = HoldPosition.Status;	46	HomePosition.Status = HoldPosition.Status;
47	if(HomePosition.Status == VALID) BeepTime = 1000; // signal if new home position was set	47	if(HomePosition.Status == VALID) BeepTime = 1000; // signal if new home position was set
48	}	48	}
49		49
50	// clear home position	50	// clear home position
51	void GPS_ClearHomePosition(void)	51	void GPS_ClearHomePosition(void)
52	{	52	{
53	HomePosition.Status = INVALID;	53	HomePosition.Status = INVALID;
54	}	54	}
55		55
56	// disable GPS control sticks	56	// disable GPS control sticks
57	void GPS_Neutral(void)	57	void GPS_Neutral(void)
58	{	58	{
59	GPS_Pitch = 0;	59	GPS_Pitch = 0;
60	GPS_Roll = 0;	60	GPS_Roll = 0;
61	}	61	}
62		62
63	// calculates the GPS control sticks values from the deviation to target position	63	// calculates the GPS control sticks values from the deviation to target position
64	void GPS_PDController(GPS_Pos_t *TargetPos)	64	void GPS_PDController(GPS_Pos_t *TargetPos)
65	{	65	{
66	int32_t coscompass, sincompass;	66	int32_t coscompass, sincompass;
67	int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0;	67	int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0;
68	int32_t PD_North = 0, PD_East = 0;	68	int32_t PD_North = 0, PD_East = 0;
69		69
70	if( (TargetPos->Status == VALID) && (GPSInfo.status == VALID) && (GPSInfo.satfix == SATFIX_3D))	70	if( (TargetPos->Status == VALID) && (GPSInfo.status == VALID) && (GPSInfo.satfix == SATFIX_3D))
71	{	71	{
72	GPSPosDev_North = GPSInfo.utmnorth - TargetPos->Northing;	72	GPSPosDev_North = GPSInfo.utmnorth - TargetPos->Northing;
73	GPSPosDev_East = GPSInfo.utmeast - TargetPos->Easting;	73	GPSPosDev_East = GPSInfo.utmeast - TargetPos->Easting;
74		74
75	DebugOut.Analog[12] = GPSPosDev_North;	75	DebugOut.Analog[12] = GPSPosDev_North;
76	DebugOut.Analog[13] = GPSPosDev_East;	76	DebugOut.Analog[13] = GPSPosDev_East;
77		77
78	//DebugOut.Analog[12] = GPSInfo.velnorth;	78	//DebugOut.Analog[12] = GPSInfo.velnorth;
79	//DebugOut.Analog[13] = GPSInfo.veleast;	79	//DebugOut.Analog[13] = GPSInfo.veleast;
80		80
81	//Calculate PD-components of the controller (negative sign for compensation)	81	//Calculate PD-components of the controller (negative sign for compensation)
82	P_North = -(GPS_P_Factor * GPSPosDev_North)/2048;	82	P_North = -(GPS_P_Factor * GPSPosDev_North)/2048;
83	D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512;	83	D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512;
84	P_East = -(GPS_P_Factor * GPSPosDev_East)/2048;	84	P_East = -(GPS_P_Factor * GPSPosDev_East)/2048;
85	D_East = -(GPS_D_Factor * GPSInfo.veleast)/512;	85	D_East = -(GPS_D_Factor * GPSInfo.veleast)/512;
86		86
87	if( (abs(GPSPosDev_North) > GPS_D_LIMIT_DIST) \|\| (abs(GPSPosDev_East) > GPS_D_LIMIT_DIST) )	87	if( (abs(GPSPosDev_North) > GPS_D_LIMIT_DIST) \|\| (abs(GPSPosDev_East) > GPS_D_LIMIT_DIST) )
88	{	88	{
89	if (D_North > GPS_D_LIMIT) D_North = GPS_D_LIMIT;	89	if (D_North > GPS_D_LIMIT) D_North = GPS_D_LIMIT;
90	else if (D_North < -GPS_D_LIMIT) D_North = -GPS_D_LIMIT;	90	else if (D_North < -GPS_D_LIMIT) D_North = -GPS_D_LIMIT;
91	if (D_East > GPS_D_LIMIT) D_East = GPS_D_LIMIT;	91	if (D_East > GPS_D_LIMIT) D_East = GPS_D_LIMIT;
92	else if (D_East < -GPS_D_LIMIT) D_East = -GPS_D_LIMIT;	92	else if (D_East < -GPS_D_LIMIT) D_East = -GPS_D_LIMIT;
93	}	93	}
94		94
95	// PD-controller	95	// PD-controller
96	PD_North = P_North + D_North;	96	PD_North = P_North + D_North;
97	PD_East = P_East + D_East;	97	PD_East = P_East + D_East;
98		98
99	// GPS to pitch and roll settings	99	// GPS to pitch and roll settings
100		100
101	//A positive pitch angle moves head downwards (flying forward).	101	//A positive pitch angle moves head downwards (flying forward).
102	//A positive roll angle tilts left side downwards (flying left).	102	//A positive roll angle tilts left side downwards (flying left).
103		103
104	// If compass heading is 0 the head of the copter is in north direction.	104	// If compass heading is 0 the head of the copter is in north direction.
105	// A positive pitch angle will fly to north and a positive roll angle will fly to west.	105	// A positive pitch angle will fly to north and a positive roll angle will fly to west.
106	// In case of a positive north deviation/velocity the	106	// In case of a positive north deviation/velocity the
107	// copter should fly to south (negative pitch).	107	// copter should fly to south (negative pitch).
108	// In case of a positive east position deviation and a positive east velocity the	108	// In case of a positive east position deviation and a positive east velocity the
109	// copter should fly to west (positive roll).	109	// copter should fly to west (positive roll).
110		110
111	// The influence of the GPS_Pitch and GPS_Roll variable is contrarily to the stick values	111	// The influence of the GPS_Pitch and GPS_Roll variable is contrarily to the stick values
112	// in the fc.c. Therefore a positive north deviation/velocity should result in a positive	112	// in the fc.c. Therefore a positive north deviation/velocity should result in a positive
113	// GPS_Pitch and a positive east deviation/velocity should result in a negative GPS_Roll.	113	// GPS_Pitch and a positive east deviation/velocity should result in a negative GPS_Roll.
114		114
115	// rotation transformation to compass heading to match any copter orientation	115	// rotation transformation to compass heading to match any copter orientation
116	if (CompassHeading < 0) // no valid compass data	116	if (CompassHeading < 0) // no valid compass data
117	{ // disable GPS	117	{ // disable GPS
118	GPS_Neutral();	118	GPS_Neutral();
119	}	119	}
120	else	120	else
121	{	121	{
122	coscompass = (int32_t)c_cos_8192(CompassHeading);	122	coscompass = (int32_t)c_cos_8192(CompassHeading);
123	sincompass = (int32_t)c_sin_8192(CompassHeading);	123	sincompass = (int32_t)c_sin_8192(CompassHeading);
124	GPS_Roll = (int16_t)((coscompass * PD_East - sincompass * PD_North) / 8192);	124	GPS_Roll = (int16_t)((coscompass * PD_East - sincompass * PD_North) / 8192);
125	GPS_Pitch = -1(int16_t)((sincompass PD_East + coscompass * PD_North) / 8192);	125	GPS_Pitch = -1(int16_t)((sincompass PD_East + coscompass * PD_North) / 8192);
126	}	126	}
127	// limit GPS controls	127	// limit GPS controls
128	if (GPS_Pitch > GPS_STICK_LIMIT) GPS_Pitch = GPS_STICK_LIMIT;	128	if (GPS_Pitch > GPS_STICK_LIMIT) GPS_Pitch = GPS_STICK_LIMIT;
129	else if (GPS_Pitch < -GPS_STICK_LIMIT) GPS_Pitch = -GPS_STICK_LIMIT;	129	else if (GPS_Pitch < -GPS_STICK_LIMIT) GPS_Pitch = -GPS_STICK_LIMIT;
130	if (GPS_Roll > GPS_STICK_LIMIT) GPS_Roll = GPS_STICK_LIMIT;	130	if (GPS_Roll > GPS_STICK_LIMIT) GPS_Roll = GPS_STICK_LIMIT;
131	else if (GPS_Roll < -GPS_STICK_LIMIT) GPS_Roll = -GPS_STICK_LIMIT;	131	else if (GPS_Roll < -GPS_STICK_LIMIT) GPS_Roll = -GPS_STICK_LIMIT;
132	}	132	}
133	else // invalid input data	133	else // invalid input data
134	{	134	{
135	BeepTime = 50;	135	BeepTime = 50;
136	GPS_Neutral();	136	GPS_Neutral();
137	}	137	}
138	}	138	}
139		139
140		140
141	void GPS_Main(uint8_t ctrl)	141	void GPS_Main(uint8_t ctrl)
142	{	142	{
143	static uint8_t GPS_Task = TSK_IDLE;	143	static uint8_t GPS_Task = TSK_IDLE;
144	int16_t satbeep;	144	int16_t satbeep;
145		145
146	// ctrl enables the gps feature	146	// ctrl enables the gps feature
147	if(ctrl < 70) GPS_Task = TSK_IDLE;	147	if(ctrl < 70) GPS_Task = TSK_IDLE;
148	else if (ctrl < 160) GPS_Task = TSK_HOLD;	148	else if (ctrl < 160) GPS_Task = TSK_HOLD;
149	else GPS_Task = TSK_HOME; // ctrl >= 160	149	else GPS_Task = TSK_HOME; // ctrl >= 160
150		150
151		151
152	switch(GPSInfo.status)	152	switch(GPSInfo.status)
153	{	153	{
154	case INVALID: // invalid gps data	154	case INVALID: // invalid gps data
155	GPS_Neutral();	155	GPS_Neutral();
156	if(GPS_Task != TSK_IDLE)	156	if(GPS_Task != TSK_IDLE)
157	{	157	{
158	BeepTime = 100; // beep if signal is neccesary	158	BeepTime = 100; // beep if signal is neccesary
159	}	159	}
160	break;	160	break;
161	case PROCESSED: // if gps data are already processed	161	case PROCESSED: // if gps data are already processed
162	// downcount timeout	162	// downcount timeout
163	if(GPSTimeout) GPSTimeout--;	163	if(GPSTimeout) GPSTimeout--;
164	// if no new data arrived within timeout set current data invalid	164	// if no new data arrived within timeout set current data invalid
165	// and therefore disable GPS	165	// and therefore disable GPS
166	else	166	else
167	{	167	{
168	GPS_Neutral();	168	GPS_Neutral();
169	GPSInfo.status = INVALID;	169	GPSInfo.status = INVALID;
170	}	170	}
171	break;	171	break;
172	case VALID: // new valid data from gps device	172	case VALID: // new valid data from gps device
173	// if the gps data quality is sufficient	173	// if the gps data quality is sufficient
174	if (GPSInfo.satfix == SATFIX_3D)	174	if (GPSInfo.satfix == SATFIX_3D)
175	{	175	{
176	switch(GPS_Task) // check what's to do	176	switch(GPS_Task) // check what's to do
177	{	177	{
178	case TSK_IDLE:	178	case TSK_IDLE:
179	// update hold point to current gps position	179	// update hold point to current gps position
180	HoldPosition.Northing = GPSInfo.utmnorth;	180	HoldPosition.Northing = GPSInfo.utmnorth;
181	HoldPosition.Easting = GPSInfo.utmeast;	181	HoldPosition.Easting = GPSInfo.utmeast;
182	HoldPosition.Status = VALID;	182	HoldPosition.Status = VALID;
183	// disable gps control	183	// disable gps control
184	GPS_Neutral();	184	GPS_Neutral();
185	break; // eof TSK_IDLE	185	break; // eof TSK_IDLE
186	case TSK_HOLD:	186	case TSK_HOLD:
187	if(HoldPosition.Status == VALID)	187	if(HoldPosition.Status == VALID)
188	{	188	{
189	// if sticks are centered (no manual control)	189	// if sticks are centered (no manual control)
190	if ((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE))	190	if ((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE))
191	{	191	{
192	GPS_PDController(&HoldPosition);	192	GPS_PDController(&HoldPosition);
193	}	193	}
194	else // MK controlled by user	194	else // MK controlled by user
195	{	195	{
196	// update hold point to current gps position	196	// update hold point to current gps position
197	HoldPosition.Northing = GPSInfo.utmnorth;	197	HoldPosition.Northing = GPSInfo.utmnorth;
198	HoldPosition.Easting = GPSInfo.utmeast;	198	HoldPosition.Easting = GPSInfo.utmeast;
199	HoldPosition.Status = GPSInfo.status;	199	HoldPosition.Status = GPSInfo.status;
200	// disable gps control	200	// disable gps control
201	GPS_Neutral();	201	GPS_Neutral();
202	}	202	}
203	}	203	}
204	break; // eof TSK_HOLD	204	break; // eof TSK_HOLD
205	case TSK_HOME:	205	case TSK_HOME:
206	if(HomePosition.Status == VALID)	206	if(HomePosition.Status == VALID)
207	{	207	{
208	// update hold point to current gps position	208	// update hold point to current gps position
209	// to avoid a flight back if home comming is deactivated	209	// to avoid a flight back if home comming is deactivated
210	HoldPosition.Northing = GPSInfo.utmnorth;	210	HoldPosition.Northing = GPSInfo.utmnorth;
211	HoldPosition.Easting = GPSInfo.utmeast;	211	HoldPosition.Easting = GPSInfo.utmeast;
212	HoldPosition.Status = GPSInfo.status;	212	HoldPosition.Status = GPSInfo.status;
213	// if sticks are centered (no manual control)	213	// if sticks are centered (no manual control)
214	if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE))	214	if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE))
215	{	215	{
216	GPS_PDController(&HomePosition);	216	GPS_PDController(&HomePosition);
217	}	217	}
218	else // manual control	218	else // manual control
219	{	219	{
220	GPS_Neutral();	220	GPS_Neutral();
221	}	221	}
222	}	222	}
223	else // bad home position	223	else // bad home position
224	{	224	{
225	BeepTime = 50; // signal invalid home position	225	BeepTime = 50; // signal invalid home position
226	// try to hold at least the position as a fallback option	226	// try to hold at least the position as a fallback option
227	// if sticks are centered (no manual control)	227	// if sticks are centered (no manual control)
228	if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE) && (HoldPosition.Status == VALID))	228	if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE) && (HoldPosition.Status == VALID))
229	{	229	{
230	GPS_PDController(&HoldPosition);	230	GPS_PDController(&HoldPosition);
231	}	231	}
232	else // manual control or no rteference position	232	else // manual control or no rteference position
233	{	233	{
234	GPS_Neutral();	234	GPS_Neutral();
235	}	235	}
236	}	236	}
237	break; // eof TSK_HOME	237	break; // eof TSK_HOME
238	default: // unhandled task	238	default: // unhandled task
239	GPS_Neutral();	239	GPS_Neutral();
240	break; // eof default	240	break; // eof default
241	} // eof switch GPS_Task	241	} // eof switch GPS_Task
242	} // eof 3D-FIX	242	} // eof 3D-FIX
243		243
244	else // no 3D-SATFIX	244	else // no 3D-SATFIX
245	{ // disable gps control	245	{ // disable gps control
246	GPS_Neutral();	246	GPS_Neutral();
247	if(GPS_Task != TSK_IDLE)	247	if(GPS_Task != TSK_IDLE)
248	{	248	{
249	satbeep = 1800 - (int16_t)GPSInfo.satnum * 225; // is zero at 8 sats	249	satbeep = 1800 - (int16_t)GPSInfo.satnum * 225; // is zero at 8 sats
250	if (satbeep < 0) satbeep = 0;	250	if (satbeep < 0) satbeep = 0;
251	BeepTime = 50 + (uint16_t)satbeep; // max 1850 * 0.1 ms =	251	BeepTime = 50 + (uint16_t)satbeep; // max 1850 * 0.1 ms =
252	}	252	}
253	}	253	}
254	// set current data as processed to avoid further calculations on the same gps data	254	// set current data as processed to avoid further calculations on the same gps data
255	GPSInfo.status = PROCESSED;	255	GPSInfo.status = PROCESSED;
256	break;	256	break;
257	} // eof GPSInfo.status	257	} // eof GPSInfo.status
258	DebugOut.Analog[14] = GPS_Pitch;	258	DebugOut.Analog[14] = GPS_Pitch;
259	DebugOut.Analog[15] = GPS_Roll;	259	DebugOut.Analog[15] = GPS_Roll;
260	}	260	}
261		261
262		262

Subversion Repositories FlightCtrl

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