Subversion Repositories FlightCtrl

Rev

Rev 2087 | Rev 2160 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 2087 Rev 2088
Line 17... Line 17...
17
  NAVI_FLIGHT_MODE_FREE,
17
  NAVI_FLIGHT_MODE_FREE,
18
  NAVI_FLIGHT_MODE_AID,
18
  NAVI_FLIGHT_MODE_AID,
19
  NAVI_FLIGHT_MODE_HOME,
19
  NAVI_FLIGHT_MODE_HOME,
20
} FlightMode_t;
20
} FlightMode_t;
Line -... Line 21...
-
 
21
 
-
 
22
/*
-
 
23
 * This is not read from internally. It only serves to let a pilot/debugger
-
 
24
 * know the status of the navigation system (thru a data connection).
21
 
25
 */
-
 
26
typedef enum {
-
 
27
  NAVI_STATUS_NO_COMPASS=-1,
-
 
28
  NAVI_STATUS_INVALID_GPS=-2,
-
 
29
  NAVI_STATUS_BAD_GPS_SIGNAL=-3,
-
 
30
  NAVI_STATUS_RTH_POSITION_INVALID=-4,
-
 
31
  NAVI_STATUS_RTH_FALLBACK_ON_HOLD=-5,
22
typedef enum {
32
  NAVI_STATUS_GPS_TIMEOUT=-6,
23
  NAVI_STATUS_FREEFLIGHT=0,
-
 
24
  NAVI_STATUS_INVALID_GPS=1,
-
 
25
  NAVI_STATUS_BAD_GPS_SIGNAL=2,
-
 
26
  NAVI_STATUS_MANUAL_OVERRIDE=5,
33
  NAVI_STATUS_FREEFLIGHT=0,
27
  NAVI_STATUS_POSITION_HOLD=7,
34
  NAVI_STATUS_POSITION_HOLD=1,
28
  NAVI_STATUS_RTH=8,
-
 
29
  NAVI_STATUS_HOLD_POSITION_INVALID=9,
35
  NAVI_STATUS_RTH=2,
30
  NAVI_STATUS_RTH_FALLBACK_ON_HOLD=10,
36
  NAVI_STATUS_MANUAL_OVERRIDE=3,
31
  NAVI_STATUS_RTH_POSITION_INVALID=11,
-
 
32
  NAVI_STATUS_GPS_TIMEOUT=12
37
  NAVI_STATUS_HOLD_POSITION_INVALID=4,
Line 33... Line 38...
33
} NaviStatus_t;
38
} NaviStatus_t;
34
 
39
 
35
#define GPS_POSINTEGRAL_LIMIT 32000
40
#define GPS_POSINTEGRAL_LIMIT 32000
Line 49... Line 54...
49
GPS_Pos_t homePosition = { 0, 0, 0, INVALID };
54
GPS_Pos_t homePosition = { 0, 0, 0, INVALID };
50
// the current flight mode
55
// the current flight mode
51
FlightMode_t flightMode = NAVI_FLIGHT_MODE_UNDEF;
56
FlightMode_t flightMode = NAVI_FLIGHT_MODE_UNDEF;
52
int16_t naviSticks[2] = {0,0};
57
int16_t naviSticks[2] = {0,0};
Line 53... Line 58...
53
 
58
 
Line 54... Line 59...
54
uint8_t naviStatus;
59
int8_t naviStatus;
55
 
60
 
56
// ---------------------------------------------------------------------------------
61
// ---------------------------------------------------------------------------------
Line 264... Line 269...
264
}
269
}
Line 265... Line 270...
265
 
270
 
266
void navigation_periodicTaskAndPRTY(int16_t* PRTY) {
271
void navigation_periodicTaskAndPRTY(int16_t* PRTY) {
267
  static uint8_t GPS_P_Delay = 0;
272
  static uint8_t GPS_P_Delay = 0;
268
  static uint16_t beep_rythm = 0;
-
 
269
 
273
  static uint16_t beep_rythm = 0;
270
  static uint16_t navi_testOscPrescaler = 0;
274
  static uint16_t navi_testOscPrescaler = 0;
Line 271... Line -...
271
  static uint8_t  navi_testOscTimer = 0;
-
 
272
 
-
 
273
  navi_testOscPrescaler++;
-
 
274
  if (navi_testOscPrescaler == 488) {
-
 
275
          navi_testOscPrescaler = 0;
275
  static uint8_t  navi_testOscTimer = 0;
276
          navi_testOscTimer++;
276
 
277
          if (navi_testOscTimer == staticParams.naviTestOscPeriod) {
-
 
278
                  navi_testOscTimer = 0;
277
  if (!(staticParams.bitConfig & CFG_COMPASS_ENABLED)) {
279
                  if (staticParams.naviTestOscAmplitude) {
-
 
280
                          holdPosition.status = NEWDATA;
278
    navi_setNeutral();
281
                          holdPosition.latitude += staticParams.naviTestOscAmplitude * 90L;
-
 
282
                  }
-
 
283
          } else if (navi_testOscTimer == staticParams.naviTestOscPeriod/2) {
-
 
284
                  if (staticParams.naviTestOscAmplitude) {
-
 
285
                          holdPosition.status = NEWDATA;
-
 
286
                          holdPosition.latitude -= staticParams.naviTestOscAmplitude * 90L;
-
 
287
                  }
279
    naviStatus = NAVI_STATUS_NO_COMPASS;
Line 288... Line 280...
288
          }
280
    return;
Line -... Line 281...
-
 
281
  }
-
 
282
 
-
 
283
  navi_updateFlightMode();
-
 
284
 
-
 
285
  navi_testOscPrescaler++;
-
 
286
  if (navi_testOscPrescaler == 488) {
-
 
287
      navi_testOscPrescaler = 0;
-
 
288
      navi_testOscTimer++;
-
 
289
      if (navi_testOscTimer == staticParams.naviTestOscPeriod) {
-
 
290
          navi_testOscTimer = 0;
-
 
291
          if (staticParams.naviTestOscAmplitude) {
-
 
292
              holdPosition.status = NEWDATA;
-
 
293
              holdPosition.latitude += staticParams.naviTestOscAmplitude * 90L; // there are about 90 units per meter.
-
 
294
          }
-
 
295
      } else if (navi_testOscTimer == staticParams.naviTestOscPeriod/2) {
-
 
296
          if (staticParams.naviTestOscAmplitude) {
-
 
297
              holdPosition.status = NEWDATA;
-
 
298
              holdPosition.latitude -= staticParams.naviTestOscAmplitude * 90L;
289
  }
299
          }
290
 
300
      }
291
  navi_updateFlightMode();
301
  }
292
 
302
 
293
  // store home position if start of flight flag is set
303
  // store home position if start of flight flag is set