WebSVN - FlightCtrl - Diff - Rev 2045 and 2046 - /branches/dongfang_FC

   GPS_FLIGHT_MODE_AID,
   GPS_FLIGHT_MODE_HOME,
 } FlightMode_t;
 #define GPS_POSINTEGRAL_LIMIT 32000
-#define GPS_STICK_LIMIT         45              // limit of gps stick control to avoid critical flight attitudes
+#define GPS_STICK_LIMIT         400             // limit of gps stick control to avoid critical flight attitudes
-#define GPS_P_LIMIT                     25
+#define GPS_P_LIMIT                     100
 typedef struct {
   int32_t longitude;
 GPS_Pos_t homePosition = { 0, 0, 0, INVALID };
 // the current flight mode
 FlightMode_t flightMode = GPS_FLIGHT_MODE_UNDEF;
 // ---------------------------------------------------------------------------------
-void GPS_updateFlightMode(void) {
+void navi_updateFlightMode(void) {
   static FlightMode_t flightModeOld = GPS_FLIGHT_MODE_UNDEF;
   if (controlMixer_getSignalQuality() <= SIGNAL_BAD
   debugOut.analog[31] = flightMode;
+}
 // ---------------------------------------------------------------------------------
 // This function defines a good GPS signal condition
-uint8_t GPS_isSignalOK(void) {
+uint8_t navi_isGPSSignalOK(void) {
   static uint8_t GPSFix = 0;
   if ((GPSInfo.status != INVALID) && (GPSInfo.satfix == SATFIX_3D)
       && (GPSInfo.flags & FLAG_GPSFIXOK)
       && ((GPSInfo.satnum >= staticParams.GPSMininumSatellites) || GPSFix)) {
     return (0);
+}
 // ---------------------------------------------------------------------------------
 // rescale xy-vector length to  limit
-uint8_t GPS_limitXY(int32_t *x, int32_t *y, int32_t limit) {
+uint8_t navi_limitXY(int32_t *x, int32_t *y, int32_t limit) {
   int32_t len;
   len = isqrt32(*x * *x + *y * *y);
   if (len > limit) {
     // normalize control vector components to the limit
+  }
   return 0;
+}
 // checks nick and roll sticks for manual control
-uint8_t GPS_isManuallyControlled(int16_t* sticks) {
+uint8_t navi_isManuallyControlled(int16_t* sticks) {
   if (sticks[CONTROL_PITCH] < staticParams.naviStickThreshold
       && sticks[CONTROL_ROLL] < staticParams.naviStickThreshold)
     return 0;
   else
     return 1;
+}
 // set given position to current gps position
-uint8_t GPS_writeCurrPositionTo(GPS_Pos_t * pGPSPos) {
+uint8_t navi_writeCurrPositionTo(GPS_Pos_t * pGPSPos) {
   if (pGPSPos == NULL)
     return 0; // bad pointer
-  if (GPS_isSignalOK()) { // is GPS signal condition is fine
+  if (navi_isGPSSignalOK()) { // is GPS signal condition is fine
     pGPSPos->longitude = GPSInfo.longitude;
     pGPSPos->latitude = GPSInfo.latitude;
     return 0;
+  }
+}
 // clear position
-uint8_t GPS_clearPosition(GPS_Pos_t * pGPSPos) {
+uint8_t navi_clearPosition(GPS_Pos_t * pGPSPos) {
   if (pGPSPos == NULL)
     return 0; // bad pointer
   else {
     pGPSPos->longitude = 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 GPS_PIDController(GPS_Pos_t *pTargetPos, int16_t* sticks) {
+void navi_PIDController(GPS_Pos_t *pTargetPos, int16_t* sticks) {
   static int32_t PID_Nick, 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;
   static int32_t cos_target_latitude = 1;
   static int32_t GPSPosDevIntegral_North = 0, GPSPosDevIntegral_East = 0;
   static GPS_Pos_t *pLastTargetPos = 0;
   // if GPS data and Compass are ok
-  if (GPS_isSignalOK() && (magneticHeading >= 0)) {
+  if (navi_isGPSSignalOK() && (magneticHeading >= 0)) {
     if (pTargetPos != NULL) { // if there is a target position
       if (pTargetPos->status != INVALID) { // and the position data are valid
         // if the target data are updated or the target pointer has changed
+    }
     //Calculate PID-components of the controller
     // D-Part
-    D_North = ((int32_t) staticParams.naviD * GPSInfo.velnorth) / 512;
+    D_North = ((int32_t) staticParams.naviD * GPSInfo.velnorth) >> 9;
-    D_East = ((int32_t) staticParams.naviD * GPSInfo.veleast) / 512;
+    D_East = ((int32_t) staticParams.naviD * GPSInfo.veleast) >> 9;
     // P-Part
-    P_North = ((int32_t) staticParams.naviP * GPSPosDev_North) / 2048;
+    P_North = ((int32_t) staticParams.naviP * GPSPosDev_North) >> 11;
-    P_East = ((int32_t) staticParams.naviP * GPSPosDev_East) / 2048;
+    P_East = ((int32_t) staticParams.naviP * GPSPosDev_East) >> 11;
     // I-Part
-    I_North = ((int32_t) staticParams.naviI * GPSPosDevIntegral_North)
+    // I-Part
-        / 8192;
+    I_North = ((int32_t) staticParams.naviI * GPSPosDevIntegral_North) >> 13;
-    I_East = ((int32_t) staticParams.naviI * GPSPosDevIntegral_East) / 8192;
+    I_East = ((int32_t) staticParams.naviI * GPSPosDevIntegral_East) >> 13;
+    // combine P & I
-    // combine P & I
+    PID_North = P_North + I_North;
-    PID_North = P_North + I_North;
+    PID_East = P_East + I_East;
-    PID_East = P_East + I_East;
-    if (!GPS_limitXY(&PID_North, &PID_East, GPS_P_LIMIT)) {
+    if (!navi_limitXY(&PID_North, &PID_East, GPS_P_LIMIT)) {
-      GPSPosDevIntegral_North += GPSPosDev_North / 16;
+      // within limit
-      GPSPosDevIntegral_East += GPSPosDev_East / 16;
+      GPSPosDevIntegral_North += GPSPosDev_North >> 4;
-      GPS_limitXY(&GPSPosDevIntegral_North, &GPSPosDevIntegral_East,
+      GPSPosDevIntegral_East += GPSPosDev_East >> 4;
     // GPSStickNick and a positive east deviation/velocity should result in a negative GPSStickRoll.
     coscompass = cos_360(yawGyroHeading / GYRO_DEG_FACTOR_YAW);
     sincompass = sin_360(yawGyroHeading / GYRO_DEG_FACTOR_YAW);
-    PID_Nick = (coscompass * PID_North + sincompass * PID_East) >> MATH_UNIT_FACTOR_LOG;
+    PID_Nick = (coscompass * PID_North + sincompass * PID_East) >> (MATH_UNIT_FACTOR_LOG-2);
-    PID_Roll = (sincompass * PID_North - coscompass * PID_East) >> MATH_UNIT_FACTOR_LOG;
+    PID_Roll = (sincompass * PID_North - coscompass * PID_East) >> (MATH_UNIT_FACTOR_LOG-2);
     // limit resulting GPS control vector
-    GPS_limitXY(&PID_Nick, &PID_Roll, GPS_STICK_LIMIT);
+    navi_limitXY(&PID_Nick, &PID_Roll, GPS_STICK_LIMIT);
-    debugOut.analog[27] = PID_Nick;
+    debugOut.analog[27] = -PID_Nick;
-    debugOut.analog[28] = PID_Roll;
+    debugOut.analog[28] = -PID_Roll;
-    sticks[CONTROL_PITCH] += PID_Nick;
+    sticks[CONTROL_PITCH] -= PID_Nick;
 void navigation_periodicTask(int16_t* sticks) {
   static uint8_t GPS_P_Delay = 0;
   static uint16_t beep_rythm = 0;
-  GPS_updateFlightMode();
+  navi_updateFlightMode();
   // store home position if start of flight flag is set
   if (MKFlags & MKFLAG_CALIBRATE) {
     MKFlags &= ~(MKFLAG_CALIBRATE);
-    if (GPS_writeCurrPositionTo(&homePosition)) {
+    if (navi_writeCurrPositionTo(&homePosition)) {
-        homePosition.latitude += 9000L;
+        homePosition.latitude += 10000L;
         beep(500);
+    }
     break;
   case NEWDATA: // new valid data from gps device
     // if the gps data quality is good
     beep_rythm++;
-    if (GPS_isSignalOK()) {
+    if (navi_isGPSSignalOK()) {
       switch (flightMode) { // check what's to do
       case GPS_FLIGHT_MODE_FREE:
         // update hold position to current gps position
-        GPS_writeCurrPositionTo(&holdPosition); // can get invalid if gps signal is bad
+        navi_writeCurrPositionTo(&holdPosition); // can get invalid if gps signal is bad
         // disable gps control
         break;
       case GPS_FLIGHT_MODE_AID:
         if (holdPosition.status != INVALID) {
-          if (GPS_isManuallyControlled(sticks)) { // MK controlled by user
+          if (navi_isManuallyControlled(sticks)) { // MK controlled by user
             // update hold point to current gps position
-            GPS_writeCurrPositionTo(&holdPosition);
+            navi_writeCurrPositionTo(&holdPosition);
             // disable gps control
             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++;
-              GPS_writeCurrPositionTo(&holdPosition); // update hold point to current gps position
+              navi_writeCurrPositionTo(&holdPosition); // update hold point to current gps position
-              GPS_PIDController(NULL, sticks); // activates only the D-Part
+              navi_PIDController(NULL, sticks); // activates only the D-Part
             } else
-              GPS_PIDController(&holdPosition, sticks); // activates the P&D-Part
+              navi_PIDController(&holdPosition, sticks); // activates the P&D-Part
+          }
         } else // invalid Hold Position
         { // try to catch a valid hold position from gps data input
-          GPS_writeCurrPositionTo(&holdPosition);
+          navi_writeCurrPositionTo(&holdPosition);
+        }
         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
-          GPS_writeCurrPositionTo(&holdPosition);
+          navi_writeCurrPositionTo(&holdPosition);
-          if (GPS_isManuallyControlled(sticks)) // MK controlled by user
+          if (navi_isManuallyControlled(sticks)) // MK controlled by user
           {
           } else {// GPS control active
-            GPS_PIDController(&homePosition, sticks);
+            navi_PIDController(&homePosition, sticks);
+          }
         } 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 (GPS_isManuallyControlled(sticks)) {
+            if (navi_isManuallyControlled(sticks)) {
               // MK controlled by user
             } else {
               // GPS control active
-              GPS_PIDController(&holdPosition, sticks);
+              navi_PIDController(&holdPosition, sticks);
+            }
           } else { // try to catch a valid hold position
-            GPS_writeCurrPositionTo(&holdPosition);
+            navi_writeCurrPositionTo(&holdPosition);
+          }
+        }

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/directGPSNaviControl.c @ 2189 - Rev 2045 → 2046