WebSVN - FlightCtrl - Diff - Rev 2057 and 2058 - /branches/dongfang_FC

   GPS_FLIGHT_MODE_AID,
   GPS_FLIGHT_MODE_HOME,
 } FlightMode_t;
 #define GPS_POSINTEGRAL_LIMIT 32000
-#define LOG_NAVI_STICK_GAIN 2
+#define LOG_NAVI_STICK_GAIN 3
 #define GPS_P_LIMIT                     100
 typedef struct {
 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;
+int16_t naviSticks[2] = {0,0};
 // ---------------------------------------------------------------------------------
 void navi_updateFlightMode(void) {
   static FlightMode_t flightModeOld = GPS_FLIGHT_MODE_UNDEF;
-  if (controlMixer_getSignalQuality() <= SIGNAL_BAD || (MKFlags & MKFLAG_EMERGENCY_FLIGHT)) {
+  if (MKFlags & MKFLAG_EMERGENCY_FLIGHT) {
-    flightMode = GPS_FLIGHT_MODE_HOME;
+    flightMode = GPS_FLIGHT_MODE_FREE;
   } else {
     if (dynamicParams.naviMode < 50)
       flightMode = GPS_FLIGHT_MODE_FREE;
     pGPSPos->status = INVALID;
+  }
   return 1;
+}
+void navi_setNeutral(void) {
+  naviSticks[CONTROL_PITCH] = naviSticks[CONTROL_ROLL] = 0;
+}
 // calculates the GPS control stick values from the deviation to target position
 // if the pointer to the target positin is NULL or is the target position invalid
 // then the P part of the controller is deactivated.
-void navi_PIDController(GPS_Pos_t *pTargetPos, int16_t *PRTY) {
+void navi_PIDController(GPS_Pos_t *pTargetPos) {
-  static int32_t PID_Nick, PID_Roll;
+  static int32_t PID_Pitch, PID_Roll;
   int32_t coscompass, sincompass;
   int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm
   int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, I_East = 0;
   int32_t PID_North = 0, PID_East = 0;
       GPSPosDevIntegral_North = 0;
       GPSPosDevIntegral_East = 0;
+    }
-    //Calculate PID-components of the controller
     //Calculate PID-components of the controller
     // D-Part
     D_North = ((int32_t) staticParams.naviD * GPSInfo.velnorth) >> 9;
     D_East = ((int32_t) staticParams.naviD * GPSInfo.veleast) >> 9;
     I_East = ((int32_t) staticParams.naviI * GPSPosDevIntegral_East) >> 13;
     // combine P & I
     PID_North = P_North + I_North;
+    PID_East = P_East + I_East;
     PID_East = P_East + I_East;
     if (!navi_limitXY(&PID_North, &PID_East, GPS_P_LIMIT)) {
       // within limit
       GPSPosDevIntegral_North += GPSPosDev_North >> 4;
       GPSPosDevIntegral_East += GPSPosDev_East >> 4;
     // GPSStickNick and a positive east deviation/velocity should result in a negative GPSStickRoll.
     coscompass = -cos_360(heading / GYRO_DEG_FACTOR_YAW);
     sincompass = -sin_360(heading / GYRO_DEG_FACTOR_YAW);
-    PID_Nick = (coscompass * PID_North + sincompass * PID_East) >> (LOG_MATH_UNIT_FACTOR-LOG_NAVI_STICK_GAIN);
+    PID_Pitch = (coscompass * PID_North + sincompass * PID_East) >> (LOG_MATH_UNIT_FACTOR-LOG_NAVI_STICK_GAIN);
     PID_Roll = (sincompass * PID_North - coscompass * PID_East) >> (LOG_MATH_UNIT_FACTOR-LOG_NAVI_STICK_GAIN);
     // limit resulting GPS control vector
+    navi_limitXY(&PID_Pitch, &PID_Roll, staticParams.naviStickLimit << LOG_NAVI_STICK_GAIN);
-    navi_limitXY(&PID_Nick, &PID_Roll, staticParams.naviStickLimit << LOG_NAVI_STICK_GAIN);
+    debugOut.analog[26] = PID_Pitch;
     debugOut.analog[27] = PID_Roll;
-    PRTY[CONTROL_PITCH] += PID_Nick;
+    naviSticks[CONTROL_PITCH] = PID_Pitch;
-    PRTY[CONTROL_ROLL]  += PID_Roll;
+    naviSticks[CONTROL_ROLL] = PID_Roll;
   } else { // invalid GPS data or bad compass reading
-  } else { // invalid GPS data or bad compass reading
+    // reset error integral
   // store home position if start of flight flag is set
   if (MKFlags & MKFLAG_CALIBRATE) {
     MKFlags &= ~(MKFLAG_CALIBRATE);
+    if (navi_writeCurrPositionTo(&homePosition)) {
-    if (navi_writeCurrPositionTo(&homePosition)) {
+      // shift north to simulate an offset.
-        homePosition.latitude += 10000L;
+        // homePosition.latitude += 10000L;
         beep(500);
+      }
+    }
+  switch (GPSInfo.status) {
-  switch (GPSInfo.status) {
+  case INVALID: // invalid gps data
-  case INVALID: // invalid gps data
+    navi_setNeutral();
     if (flightMode != GPS_FLIGHT_MODE_FREE) {
       beep(100); // beep if signal is neccesary
+    }
     if (GPSTimeout)
       GPSTimeout--;
     // if no new data arrived within timeout set current data invalid
     // and therefore disable GPS
     else {
+      navi_setNeutral();
       GPSInfo.status = INVALID;
+    }
     break;
   case NEWDATA: // new valid data from gps device
     // if the gps data quality is good
       switch (flightMode) { // check what's to do
       case GPS_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();
         break;
       case GPS_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
-            // disable gps control
+            navi_setNeutral();
             GPS_P_Delay = 0;
           } 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, PRTY); // activates only the D-Part
+              navi_PIDController(NULL); // activates only the D-Part
             } else
-              navi_PIDController(&holdPosition, PRTY); // activates the P&D-Part
+              navi_PIDController(&holdPosition); // activates the P&D-Part
           }
-        } else // invalid Hold Position
+        } else { // invalid Hold Position
-        { // try to catch a valid hold position from gps data input
+        // try to catch a valid hold position from gps data input
+          navi_writeCurrPositionTo(&holdPosition);
-          navi_writeCurrPositionTo(&holdPosition);
+          navi_setNeutral();
+        }
         break;
       case GPS_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
+          if (navi_isManuallyControlled(PRTY)) { // MK controlled by user
-          {
+            navi_setNeutral();
           } else {// GPS control active
-            navi_PIDController(&homePosition, PRTY);
+            navi_PIDController(&homePosition);
+          }
         } else {
           // bad home position
           beep(50); // signal invalid home position
           // try to hold at least the position as a fallback option
+          if (holdPosition.status != INVALID) {
-          if (holdPosition.status != INVALID) {
+            if (navi_isManuallyControlled(PRTY)) {
-            if (navi_isManuallyControlled(PRTY)) {
+              // MK controlled by user
-              // MK controlled by user
+              navi_setNeutral();
             } else {
               // GPS control active
-              navi_PIDController(&holdPosition, PRTY);
+              navi_PIDController(&holdPosition);
+            }
           } else { // try to catch a valid hold position
-          } else { // try to catch a valid hold position
+            navi_writeCurrPositionTo(&holdPosition);
-            navi_writeCurrPositionTo(&holdPosition);
+            navi_setNeutral();
+          }
+        }
         break; // eof TSK_HOME
-        break; // eof TSK_HOME
+      default: // unhandled task
-      default: // unhandled task
+        navi_setNeutral();
         break; // eof default
       } // eof switch GPS_Task
+    } // eof gps data quality is good
-    } // eof gps data quality is good
+    else { // gps data quality is bad
-    else // gps data quality is bad
+      // disable gps control
-    { // disable gps control
+      navi_setNeutral();
       if (flightMode != GPS_FLIGHT_MODE_FREE) {
         // beep if signal is not sufficient
+    }
     // set current data as processed to avoid further calculations on the same gps data
     GPSInfo.status = PROCESSED;
     break;
   } // eof GPSInfo.status
+  PRTY[CONTROL_PITCH] += naviSticks[CONTROL_PITCH];
+  PRTY[CONTROL_ROLL] += naviSticks[CONTROL_ROLL];
+}

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/directGPSNaviControl.c @ 2071 - Rev 2057 → 2058