Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1612 dongfang 1
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2
// + Copyright (c) 04.2007 Holger Buss
3
// + Nur für den privaten Gebrauch
4
// + www.MikroKopter.com
5
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
6
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
7
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
8
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
9
// + bzgl. der Nutzungsbedingungen aufzunehmen.
10
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
11
// + Verkauf von Luftbildaufnahmen, usw.
12
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
13
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
14
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
15
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
16
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
17
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
18
// + eindeutig als Ursprung verlinkt werden
19
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
20
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
21
// + Benutzung auf eigene Gefahr
22
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
23
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
24
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
25
// + mit unserer Zustimmung zulässig
26
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
27
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
28
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
29
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
30
// + this list of conditions and the following disclaimer.
31
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
32
// +     from this software without specific prior written permission.
33
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
34
// +     for non-commercial use (directly or indirectly)
1868 - 35
// +     Commercial use (for example: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
1612 dongfang 36
// +     with our written permission
37
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
38
// +     clearly linked as origin
39
// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
40
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
41
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
42
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
43
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
44
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
45
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
46
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
47
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
48
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
49
// +  POSSIBILITY OF SUCH DAMAGE.
50
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
51
#include <inttypes.h>
52
#include <stdlib.h>
1775 - 53
//#include "mymath.h"
1612 dongfang 54
#include "timer0.h"
55
#include "uart0.h"
56
#include "rc.h"
57
#include "eeprom.h"
58
 
1805 - 59
typedef enum {
60
        GPS_FLIGHT_MODE_UNDEF,
61
        GPS_FLIGHT_MODE_FREE,
62
        GPS_FLIGHT_MODE_AID,
63
        GPS_FLIGHT_MODE_HOME,
64
} FlightMode_t;
1612 dongfang 65
 
66
#define GPS_POSINTEGRAL_LIMIT 32000
67
#define GPS_STICK_LIMIT         45              // limit of gps stick control to avoid critical flight attitudes
68
#define GPS_P_LIMIT                     25
69
 
1805 - 70
typedef struct {
71
        int32_t Longitude;
72
        int32_t Latitude;
73
        int32_t Altitude;
74
        Status_t Status;
1612 dongfang 75
} GPS_Pos_t;
76
 
77
// GPS coordinates for hold position
1805 - 78
GPS_Pos_t HoldPosition = { 0, 0, 0, INVALID };
1612 dongfang 79
// GPS coordinates for home position
1805 - 80
GPS_Pos_t HomePosition = { 0, 0, 0, INVALID };
1612 dongfang 81
// the current flight mode
82
FlightMode_t FlightMode = GPS_FLIGHT_MODE_UNDEF;
83
 
84
// ---------------------------------------------------------------------------------
1775 - 85
void GPS_UpdateParameter(void) {
1805 - 86
        static FlightMode_t FlightModeOld = GPS_FLIGHT_MODE_UNDEF;
1612 dongfang 87
 
1805 - 88
        if ((RC_Quality < 100) || (MKFlags & MKFLAG_EMERGENCY_LANDING)) {
89
                FlightMode = GPS_FLIGHT_MODE_FREE;
90
        } else {
91
                if (dynamicParams.NaviGpsModeControl < 50)
92
                        FlightMode = GPS_FLIGHT_MODE_AID;
93
                else if (dynamicParams.NaviGpsModeControl < 180)
94
                        FlightMode = GPS_FLIGHT_MODE_FREE;
95
                else
96
                        FlightMode = GPS_FLIGHT_MODE_HOME;
97
        }
98
 
99
        if (FlightMode != FlightModeOld) {
100
                BeepTime = 100;
101
        }
102
        FlightModeOld = FlightMode;
1612 dongfang 103
}
104
 
105
// ---------------------------------------------------------------------------------
106
// This function defines a good GPS signal condition
1775 - 107
uint8_t GPS_IsSignalOK(void) {
1805 - 108
        static uint8_t GPSFix = 0;
109
        if ((GPSInfo.status != INVALID) && (GPSInfo.satfix == SATFIX_3D)
110
                        && (GPSInfo.flags & FLAG_GPSFIXOK) && ((GPSInfo.satnum
111
                        >= staticParams.NaviGpsMinSat) || GPSFix)) {
112
                GPSFix = 1;
113
                return 1;
114
        } else
115
                return (0);
1775 - 116
}
1612 dongfang 117
 
118
// ---------------------------------------------------------------------------------
119
// rescale xy-vector length to  limit
1775 - 120
uint8_t GPS_LimitXY(int32_t *x, int32_t *y, int32_t limit) {
1805 - 121
        uint8_t retval = 0;
122
        int32_t len;
123
        len = (int32_t) c_sqrt(*x * *x + *y * *y);
124
        if (len > limit) {
125
                // normalize control vector components to the limit
126
                *x = (*x * limit) / len;
127
                *y = (*y * limit) / len;
128
                retval = 1;
129
        }
130
        return (retval);
1612 dongfang 131
}
132
 
133
// checks nick and roll sticks for manual control
1775 - 134
uint8_t GPS_IsManualControlled(void) {
1805 - 135
        if ((abs(PPM_in[staticParams.ChannelAssignment[CH_NICK]])
136
                        < staticParams.NaviStickThreshold)
137
                        && (abs(PPM_in[staticParams.ChannelAssignment[CH_ROLL]])
138
                                        < staticParams.NaviStickThreshold))
139
                return 0;
140
        else
141
                return 1;
1612 dongfang 142
}
143
 
144
// set given position to current gps position
1775 - 145
uint8_t GPS_SetCurrPosition(GPS_Pos_t * pGPSPos) {
1805 - 146
        uint8_t retval = 0;
147
        if (pGPSPos == NULL)
148
                return (retval); // bad pointer
1612 dongfang 149
 
1805 - 150
        if (GPS_IsSignalOK()) { // is GPS signal condition is fine
151
                pGPSPos->Longitude = GPSInfo.longitude;
152
                pGPSPos->Latitude = GPSInfo.latitude;
153
                pGPSPos->Altitude = GPSInfo.altitude;
154
                pGPSPos->Status = NEWDATA;
155
                retval = 1;
156
        } else { // bad GPS signal condition
157
                pGPSPos->Status = INVALID;
158
                retval = 0;
159
        }
160
        return (retval);
1612 dongfang 161
}
162
 
163
// clear position
1775 - 164
uint8_t GPS_ClearPosition(GPS_Pos_t * pGPSPos) {
1805 - 165
        uint8_t retval = 0;
166
        if (pGPSPos == NULL)
167
                return retval; // bad pointer
168
        else {
169
                pGPSPos->Longitude = 0;
170
                pGPSPos->Latitude = 0;
171
                pGPSPos->Altitude = 0;
172
                pGPSPos->Status = INVALID;
173
                retval = 1;
174
        }
175
        return (retval);
1612 dongfang 176
}
177
 
178
// disable GPS control sticks
1775 - 179
void GPS_Neutral(void) {
1805 - 180
        GPSStickNick = 0;
181
        GPSStickRoll = 0;
1612 dongfang 182
}
183
 
184
// calculates the GPS control stick values from the deviation to target position
185
// if the pointer to the target positin is NULL or is the target position invalid
186
// then the P part of the controller is deactivated.
1775 - 187
void GPS_PIDController(GPS_Pos_t *pTargetPos) {
1805 - 188
        static int32_t PID_Nick, PID_Roll;
189
        int32_t coscompass, sincompass;
190
        int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm
191
        int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0,
192
                        I_East = 0;
193
        int32_t PID_North = 0, PID_East = 0;
194
        static int32_t cos_target_latitude = 1;
195
        static int32_t GPSPosDevIntegral_North = 0, GPSPosDevIntegral_East = 0;
196
        static GPS_Pos_t *pLastTargetPos = 0;
1612 dongfang 197
 
1805 - 198
        // if GPS data and Compass are ok
199
        if (GPS_IsSignalOK() && (CompassHeading >= 0)) {
200
                if (pTargetPos != NULL) // if there is a target position
201
                {
202
                        if (pTargetPos->Status != INVALID) // and the position data are valid
203
                        {
204
                                // if the target data are updated or the target pointer has changed
1821 - 205
                                if ((pTargetPos->Status != PROCESSED) || (pTargetPos != pLastTargetPos)) {
1805 - 206
                                        // reset error integral
207
                                        GPSPosDevIntegral_North = 0;
208
                                        GPSPosDevIntegral_East = 0;
209
                                        // recalculate latitude projection
210
                                        cos_target_latitude = (int32_t) c_cos_8192(
211
                                                        (int16_t) (pTargetPos->Latitude / 10000000L));
212
                                        // remember last target pointer
213
                                        pLastTargetPos = pTargetPos;
214
                                        // mark data as processed
215
                                        pTargetPos->Status = PROCESSED;
216
                                }
217
                                // calculate position deviation from latitude and longitude differences
218
                                GPSPosDev_North = (GPSInfo.latitude - pTargetPos->Latitude); // to calculate real cm we would need *111/100 additionally
219
                                GPSPosDev_East = (GPSInfo.longitude - pTargetPos->Longitude); // to calculate real cm we would need *111/100 additionally
220
                                // calculate latitude projection
221
                                GPSPosDev_East *= cos_target_latitude;
222
                                GPSPosDev_East /= 8192;
223
                        } else {// no valid target position available
224
                                // reset error
225
                                GPSPosDev_North = 0;
226
                                GPSPosDev_East = 0;
227
                                // reset error integral
228
                                GPSPosDevIntegral_North = 0;
229
                                GPSPosDevIntegral_East = 0;
230
                        }
231
                } else { // no target position available
232
                        // reset error
233
                        GPSPosDev_North = 0;
234
                        GPSPosDev_East = 0;
235
                        // reset error integral
236
                        GPSPosDevIntegral_North = 0;
237
                        GPSPosDevIntegral_East = 0;
238
                }
239
 
240
                //Calculate PID-components of the controller
241
 
242
                // D-Part
243
                D_North = ((int32_t) dynamicParams.NaviGpsD * GPSInfo.velnorth) / 512;
244
                D_East = ((int32_t) dynamicParams.NaviGpsD * GPSInfo.veleast) / 512;
245
 
246
                // P-Part
247
                P_North = ((int32_t) dynamicParams.NaviGpsP * GPSPosDev_North) / 2048;
248
                P_East = ((int32_t) dynamicParams.NaviGpsP * GPSPosDev_East) / 2048;
249
 
250
                // I-Part
251
                I_North = ((int32_t) dynamicParams.NaviGpsI * GPSPosDevIntegral_North)
252
                                / 8192;
1821 - 253
                I_East = ((int32_t) dynamicParams.NaviGpsI * GPSPosDevIntegral_East) / 8192;
1805 - 254
 
255
                // combine P & I
256
                PID_North = P_North + I_North;
257
                PID_East = P_East + I_East;
258
                if (!GPS_LimitXY(&PID_North, &PID_East, GPS_P_LIMIT)) {
259
                        GPSPosDevIntegral_North += GPSPosDev_North / 16;
260
                        GPSPosDevIntegral_East += GPSPosDev_East / 16;
261
                        GPS_LimitXY(&GPSPosDevIntegral_North, &GPSPosDevIntegral_East,
262
                                        GPS_POSINTEGRAL_LIMIT);
263
                }
264
 
265
                // combine PI- and D-Part
266
                PID_North += D_North;
267
                PID_East += D_East;
268
 
269
                // scale combination with gain.
270
                PID_North = (PID_North * (int32_t) dynamicParams.NaviGpsGain) / 100;
271
                PID_East = (PID_East * (int32_t) dynamicParams.NaviGpsGain) / 100;
272
 
273
                // GPS to nick and roll settings
274
                // A positive nick angle moves head downwards (flying forward).
275
                // A positive roll angle tilts left side downwards (flying left).
276
                // If compass heading is 0 the head of the copter is in north direction.
277
                // A positive nick angle will fly to north and a positive roll angle will fly to west.
278
                // In case of a positive north deviation/velocity the
279
                // copter should fly to south (negative nick).
280
                // In case of a positive east position deviation and a positive east velocity the
281
                // copter should fly to west (positive roll).
282
                // The influence of the GPSStickNick and GPSStickRoll variable is contrarily to the stick values
283
                // in the flight.c. Therefore a positive north deviation/velocity should result in a positive
284
                // GPSStickNick and a positive east deviation/velocity should result in a negative GPSStickRoll.
285
 
286
                coscompass = (int32_t) c_cos_8192(YawGyroHeading / GYRO_DEG_FACTOR);
287
                sincompass = (int32_t) c_sin_8192(YawGyroHeading / GYRO_DEG_FACTOR);
288
                PID_Nick = (coscompass * PID_North + sincompass * PID_East) / 8192;
289
                PID_Roll = (sincompass * PID_North - coscompass * PID_East) / 8192;
290
 
291
                // limit resulting GPS control vector
292
                GPS_LimitXY(&PID_Nick, &PID_Roll, GPS_STICK_LIMIT);
293
 
294
                GPSStickNick = (int16_t) PID_Nick;
295
                GPSStickRoll = (int16_t) PID_Roll;
296
        } else { // invalid GPS data or bad compass reading
297
                GPS_Neutral(); // do nothing
1775 - 298
                // reset error integral
299
                GPSPosDevIntegral_North = 0;
300
                GPSPosDevIntegral_East = 0;
1805 - 301
        }
1612 dongfang 302
}
303
 
1775 - 304
void GPS_Main(void) {
1805 - 305
        static uint8_t GPS_P_Delay = 0;
306
        static uint16_t beep_rythm = 0;
1612 dongfang 307
 
1805 - 308
        GPS_UpdateParameter();
1612 dongfang 309
 
1805 - 310
        // store home position if start of flight flag is set
311
        if (MKFlags & MKFLAG_CALIBRATE) {
312
                if (GPS_SetCurrPosition(&HomePosition))
313
                        BeepTime = 700;
314
        }
315
 
316
        switch (GPSInfo.status) {
317
        case INVALID: // invalid gps data
1775 - 318
                GPS_Neutral();
1805 - 319
                if (FlightMode != GPS_FLIGHT_MODE_FREE) {
320
                        BeepTime = 100; // beep if signal is neccesary
321
                }
322
                break;
323
        case PROCESSED: // if gps data are already processed do nothing
324
                // downcount timeout
325
                if (GPSTimeout)
326
                        GPSTimeout--;
327
                // if no new data arrived within timeout set current data invalid
328
                // and therefore disable GPS
329
                else {
1775 - 330
                        GPS_Neutral();
1805 - 331
                        GPSInfo.status = INVALID;
332
                }
333
                break;
334
        case NEWDATA: // new valid data from gps device
335
                // if the gps data quality is good
336
                beep_rythm++;
337
                if (GPS_IsSignalOK()) {
338
                        switch (FlightMode) { // check what's to do
339
                        case GPS_FLIGHT_MODE_FREE:
340
                                // update hold position to current gps position
341
                                GPS_SetCurrPosition(&HoldPosition); // can get invalid if gps signal is bad
342
                                // disable gps control
343
                                GPS_Neutral();
344
                                break;
345
 
346
                        case GPS_FLIGHT_MODE_AID:
347
                                if (HoldPosition.Status != INVALID) {
348
                                        if (GPS_IsManualControlled()) { // MK controlled by user
349
                                                // update hold point to current gps position
350
                                                GPS_SetCurrPosition(&HoldPosition);
351
                                                // disable gps control
352
                                                GPS_Neutral();
353
                                                GPS_P_Delay = 0;
354
                                        } else { // GPS control active
355
                                                if (GPS_P_Delay < 7) {
356
                                                        // delayed activation of P-Part for 8 cycles (8*0.25s = 2s)
357
                                                        GPS_P_Delay++;
358
                                                        GPS_SetCurrPosition(&HoldPosition); // update hold point to current gps position
359
                                                        GPS_PIDController(NULL); // activates only the D-Part
360
                                                } else
361
                                                        GPS_PIDController(&HoldPosition);// activates the P&D-Part
362
                                        }
363
                                } else // invalid Hold Position
364
                                { // try to catch a valid hold position from gps data input
365
                                        GPS_SetCurrPosition(&HoldPosition);
366
                                        GPS_Neutral();
367
                                }
368
                                break;
369
 
370
                        case GPS_FLIGHT_MODE_HOME:
371
                                if (HomePosition.Status != INVALID) {
372
                                        // update hold point to current gps position
373
                                        // to avoid a flight back if home comming is deactivated
374
                                        GPS_SetCurrPosition(&HoldPosition);
375
                                        if (GPS_IsManualControlled()) // MK controlled by user
376
                                        {
377
                                                GPS_Neutral();
378
                                        } else // GPS control active
379
                                        {
380
                                                GPS_PIDController(&HomePosition);
381
                                        }
382
                                } else // bad home position
383
                                {
384
                                        BeepTime = 50; // signal invalid home position
385
                                        // try to hold at least the position as a fallback option
386
 
387
                                        if (HoldPosition.Status != INVALID) {
388
                                                if (GPS_IsManualControlled()) // MK controlled by user
389
                                                {
390
                                                        GPS_Neutral();
391
                                                } else // GPS control active
392
                                                {
393
                                                        GPS_PIDController(&HoldPosition);
394
                                                }
395
                                        } else { // try to catch a valid hold position
396
                                                GPS_SetCurrPosition(&HoldPosition);
397
                                                GPS_Neutral();
398
                                        }
399
                                }
400
                                break; // eof TSK_HOME
401
                        default: // unhandled task
402
                                GPS_Neutral();
403
                                break; // eof default
404
                        } // eof switch GPS_Task
405
                } // eof gps data quality is good
406
                else // gps data quality is bad
407
                { // disable gps control
408
                        GPS_Neutral();
409
                        if (FlightMode != GPS_FLIGHT_MODE_FREE) {
410
                                // beep if signal is not sufficient
411
                                if (!(GPSInfo.flags & FLAG_GPSFIXOK) && !(beep_rythm % 5))
412
                                        BeepTime = 100;
1821 - 413
                                else if (GPSInfo.satnum < staticParams.NaviGpsMinSat && !(beep_rythm
414
                                                % 5))
1805 - 415
                                        BeepTime = 10;
416
                        }
417
                }
418
                // set current data as processed to avoid further calculations on the same gps data
419
                GPSInfo.status = PROCESSED;
420
                break;
421
        } // eof GPSInfo.status
1612 dongfang 422
}
423