Subversion Repositories FlightCtrl

Rev

Rev 2087 | 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