Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

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