WebSVN - FlightCtrl - Diff - Rev 2071 and 2074 - /branches/dongfang_FC

 #include "attitude.h"
 #include "dongfangMath.h"
 #include "attitude.h"
 typedef enum {
-  GPS_FLIGHT_MODE_UNDEF,
+  NAVI_FLIGHT_MODE_UNDEF,
-  GPS_FLIGHT_MODE_FREE,
+  NAVI_FLIGHT_MODE_FREE,
-  GPS_FLIGHT_MODE_AID,
+  NAVI_FLIGHT_MODE_AID,
-  GPS_FLIGHT_MODE_HOME,
+  NAVI_FLIGHT_MODE_HOME,
+} FlightMode_t;
+typedef enum {
+  NAVI_STATUS_FREEFLIGHT=0,
+  NAVI_STATUS_INVALID_GPS=1,
+  NAVI_STATUS_BAD_GPS_SIGNAL=2,
+  NAVI_STATUS_MANUAL_OVERRIDE=5,
+  NAVI_STATUS_POSITION_HOLD=7,
+  NAVI_STATUS_RTH=8,
+  NAVI_STATUS_HOLD_POSITION_INVALID=9,
+  NAVI_STATUS_RTH_FALLBACK_ON_HOLD=10,
+  NAVI_STATUS_RTH_POSITION_INVALID=11,
+  NAVI_STATUS_GPS_TIMEOUT=12
-} FlightMode_t;
+} NaviStatus_t;
 #define GPS_POSINTEGRAL_LIMIT 32000
 #define LOG_NAVI_STICK_GAIN 3
 // GPS coordinates for hold position
 GPS_Pos_t holdPosition = { 0, 0, 0, INVALID };
 // GPS coordinates for home position
 GPS_Pos_t homePosition = { 0, 0, 0, INVALID };
 // the current flight mode
-FlightMode_t flightMode = GPS_FLIGHT_MODE_UNDEF;
+FlightMode_t flightMode = NAVI_FLIGHT_MODE_UNDEF;
 int16_t naviSticks[2] = {0,0};
+uint8_t naviStatus;
 // ---------------------------------------------------------------------------------
 void navi_updateFlightMode(void) {
-  static FlightMode_t flightModeOld = GPS_FLIGHT_MODE_UNDEF;
+  static FlightMode_t flightModeOld = NAVI_FLIGHT_MODE_UNDEF;
   if (MKFlags & MKFLAG_EMERGENCY_FLIGHT) {
-    flightMode = GPS_FLIGHT_MODE_FREE;
+    flightMode = NAVI_FLIGHT_MODE_FREE;
   } else {
     if (dynamicParams.naviMode < 50)
-      flightMode = GPS_FLIGHT_MODE_FREE;
+      flightMode = NAVI_FLIGHT_MODE_FREE;
     else if (dynamicParams.naviMode < 180)
-      flightMode = GPS_FLIGHT_MODE_AID;
+      flightMode = NAVI_FLIGHT_MODE_AID;
     else
-      flightMode = GPS_FLIGHT_MODE_HOME;
+      flightMode = NAVI_FLIGHT_MODE_HOME;
+  }
+    }
   switch (GPSInfo.status) {
   case INVALID: // invalid gps data
+    navi_setNeutral();
-    navi_setNeutral();
+    naviStatus = NAVI_STATUS_INVALID_GPS;
-    if (flightMode != GPS_FLIGHT_MODE_FREE) {
+    if (flightMode != NAVI_FLIGHT_MODE_FREE) {
       beep(1); // beep if signal is neccesary
+    }
     break;
   case PROCESSED: // if gps data are already processed do nothing
     // if no new data arrived within timeout set current data invalid
     // and therefore disable GPS
     else {
       navi_setNeutral();
       GPSInfo.status = INVALID;
+      naviStatus = NAVI_STATUS_GPS_TIMEOUT;
+    }
     break;
   case NEWDATA: // new valid data from gps device
     // if the gps data quality is good
     beep_rythm++;
     if (navi_isGPSSignalOK()) {
       switch (flightMode) { // check what's to do
-      case GPS_FLIGHT_MODE_FREE:
+      case NAVI_FLIGHT_MODE_FREE:
         // update hold position to current gps position
         navi_writeCurrPositionTo(&holdPosition); // can get invalid if gps signal is bad
         // disable gps control
         navi_setNeutral();
+        naviStatus = NAVI_STATUS_FREEFLIGHT;
         break;
-      case GPS_FLIGHT_MODE_AID:
+      case NAVI_FLIGHT_MODE_AID:
         if (holdPosition.status != INVALID) {
           if (navi_isManuallyControlled(PRTY)) { // MK controlled by user
             // update hold point to current gps position
             navi_writeCurrPositionTo(&holdPosition);
             // disable gps control
             navi_setNeutral();
+            GPS_P_Delay = 0;
-            GPS_P_Delay = 0;
+            naviStatus = NAVI_STATUS_MANUAL_OVERRIDE;
           } else { // GPS control active
             if (GPS_P_Delay < 7) {
               // delayed activation of P-Part for 8 cycles (8*0.25s = 2s)
               GPS_P_Delay++;
               navi_writeCurrPositionTo(&holdPosition); // update hold point to current gps position
+              navi_PIDController(NULL); // activates only the D-Part
-              navi_PIDController(NULL); // activates only the D-Part
+              naviStatus = NAVI_STATUS_POSITION_HOLD;
-            } else
+            } else {
+              navi_PIDController(&holdPosition); // activates the P&D-Part
+              naviStatus = NAVI_STATUS_POSITION_HOLD;
               navi_PIDController(&holdPosition); // activates the P&D-Part
+          }
         } else { // invalid Hold Position
         // try to catch a valid hold position from gps data input
           navi_writeCurrPositionTo(&holdPosition);
+          navi_setNeutral();
-          navi_setNeutral();
+          naviStatus = NAVI_STATUS_HOLD_POSITION_INVALID;
+        }
         break;
-      case GPS_FLIGHT_MODE_HOME:
+      case NAVI_FLIGHT_MODE_HOME:
         if (homePosition.status != INVALID) {
           // update hold point to current gps position
           // to avoid a flight back if home comming is deactivated
           navi_writeCurrPositionTo(&holdPosition);
           if (navi_isManuallyControlled(PRTY)) { // MK controlled by user
             navi_setNeutral();
           } else {// GPS control active
+            navi_PIDController(&homePosition);
             navi_PIDController(&homePosition);
           naviStatus = NAVI_STATUS_RTH;
         } else {
           // bad home position
-          beep(50); // signal invalid home position
-          // try to hold at least the position as a fallback option
+          beep(50); // signal invalid home position
           // try to hold at least the position as a fallback option
           if (holdPosition.status != INVALID) {
             if (navi_isManuallyControlled(PRTY)) {
+              // MK controlled by user
-              // MK controlled by user
+              navi_setNeutral();
-              navi_setNeutral();
+              naviStatus = NAVI_STATUS_MANUAL_OVERRIDE;
             } else {
+              // GPS control active
-              // GPS control active
+              navi_PIDController(&holdPosition);
-              navi_PIDController(&holdPosition);
+              naviStatus = NAVI_STATUS_RTH_FALLBACK_ON_HOLD;
+            }
+          } else { // try to catch a valid hold position
-          } else { // try to catch a valid hold position
+            navi_writeCurrPositionTo(&holdPosition);
-            navi_writeCurrPositionTo(&holdPosition);
+            naviStatus = NAVI_STATUS_RTH_POSITION_INVALID;
             navi_setNeutral();
+          }
+        }
       } // eof switch GPS_Task
     } // eof gps data quality is good
     else { // gps data quality is bad
       // disable gps control
       navi_setNeutral();
+      naviStatus = NAVI_STATUS_BAD_GPS_SIGNAL;
-      if (flightMode != GPS_FLIGHT_MODE_FREE) {
+      if (flightMode != NAVI_FLIGHT_MODE_FREE) {
         // beep if signal is not sufficient
         if (!(GPSInfo.flags & FLAG_GPSFIXOK) && !(beep_rythm % 5))
           beep(100);
         else if (GPSInfo.satnum < staticParams.GPSMininumSatellites
             && !(beep_rythm % 5))
     break;
   } // eof GPSInfo.status
   PRTY[CONTROL_PITCH] += naviSticks[CONTROL_PITCH];
+  PRTY[CONTROL_ROLL] += naviSticks[CONTROL_ROLL];
+  debugOut.analog[16] = flightMode;
+  debugOut.analog[17] = naviStatus;
+  debugOut.analog[18] = naviSticks[CONTROL_PITCH];
-  PRTY[CONTROL_ROLL] += naviSticks[CONTROL_ROLL];
+  debugOut.analog[19] = naviSticks[CONTROL_ROLL];

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/directGPSNaviControl.c @ 2097 - Rev 2071 → 2074