Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

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