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

 void GPS_updateFlightMode(void) {
   static FlightMode_t flightModeOld = GPS_FLIGHT_MODE_UNDEF;
   if (controlMixer_getSignalQuality() <= SIGNAL_BAD
       || MKFlags & MKFLAG_EMERGENCY_FLIGHT) {
-    flightMode = GPS_FLIGHT_MODE_FREE;
+    flightMode = GPS_FLIGHT_MODE_HOME;
   } else {
-    if (dynamicParams.directGPSMode < 50)
-      flightMode = GPS_FLIGHT_MODE_AID;
-    else if (dynamicParams.directGPSMode < 180)
+    if (dynamicParams.naviMode < 50)
+      flightMode = GPS_FLIGHT_MODE_FREE;
+    else if (dynamicParams.naviMode < 180)
-      flightMode = GPS_FLIGHT_MODE_FREE;
+      flightMode = GPS_FLIGHT_MODE_AID;
     else
       flightMode = GPS_FLIGHT_MODE_HOME;
+  }
   if (flightMode != flightModeOld) {
     beep(100);
+    flightModeOld = flightMode;
+  }
     flightModeOld = flightMode;
   debugOut.analog[31] = flightMode;
+}
   else
     return 1;
+}
 // set given position to current gps position
-uint8_t GPS_setCurrPosition(GPS_Pos_t * pGPSPos) {
+uint8_t GPS_writeCurrPositionTo(GPS_Pos_t * pGPSPos) {
   if (pGPSPos == NULL)
     return 0; // bad pointer
             || (pTargetPos != pLastTargetPos)) {
           // reset error integral
           GPSPosDevIntegral_North = 0;
           GPSPosDevIntegral_East = 0;
           // recalculate latitude projection
-          cos_target_latitude = cos_360((int16_t) (pTargetPos->latitude / 10000000L));
+          cos_target_latitude = cos_360(pTargetPos->latitude / 10000000L);
           // remember last target pointer
           pLastTargetPos = pTargetPos;
           // mark data as processed
           pTargetPos->status = PROCESSED;
+        }
     PID_Roll = (sincompass * PID_North - coscompass * PID_East) >> MATH_UNIT_FACTOR_LOG;
     // limit resulting GPS control vector
+    GPS_limitXY(&PID_Nick, &PID_Roll, GPS_STICK_LIMIT);
+    debugOut.analog[27] = PID_Nick;
-    GPS_limitXY(&PID_Nick, &PID_Roll, GPS_STICK_LIMIT);
+    debugOut.analog[28] = PID_Roll;
+    sticks[CONTROL_PITCH] += PID_Nick;
-    sticks[CONTROL_PITCH] += (int16_t) PID_Nick;
+    sticks[CONTROL_ROLL]  += PID_Roll;
     sticks[CONTROL_ROLL] += (int16_t) PID_Roll;
   } else { // invalid GPS data or bad compass reading
     // reset error integral
     GPSPosDevIntegral_North = 0;
   GPS_updateFlightMode();
   // store home position if start of flight flag is set
   if (MKFlags & MKFLAG_CALIBRATE) {
     MKFlags &= ~(MKFLAG_CALIBRATE);
+    if (GPS_writeCurrPositionTo(&homePosition)) {
-    if (GPS_setCurrPosition(&homePosition))
+        homePosition.latitude += 9000L;
+        beep(500);
-      beep(500);
+      }
+    }
   switch (GPSInfo.status) {
   case INVALID: // invalid gps data
     beep_rythm++;
     if (GPS_isSignalOK()) {
       switch (flightMode) { // check what's to do
       case GPS_FLIGHT_MODE_FREE:
         // update hold position to current gps position
-        GPS_setCurrPosition(&holdPosition); // can get invalid if gps signal is bad
+        GPS_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
             // update hold point to current gps position
-            GPS_setCurrPosition(&holdPosition);
+            GPS_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_setCurrPosition(&holdPosition); // update hold point to current gps position
+              GPS_writeCurrPositionTo(&holdPosition); // update hold point to current gps position
               GPS_PIDController(NULL, sticks); // activates only the D-Part
             } else
               GPS_PIDController(&holdPosition, sticks); // activates the P&D-Part
+          }
         } else // invalid Hold Position
         { // try to catch a valid hold position from gps data input
-          GPS_setCurrPosition(&holdPosition);
+          GPS_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_setCurrPosition(&holdPosition);
+          GPS_writeCurrPositionTo(&holdPosition);
           if (GPS_isManuallyControlled(sticks)) // MK controlled by user
+          {
           } else {// GPS control active
             } else {
               // GPS control active
               GPS_PIDController(&holdPosition, sticks);
+            }
           } else { // try to catch a valid hold position
-            GPS_setCurrPosition(&holdPosition);
+            GPS_writeCurrPositionTo(&holdPosition);
+          }
+        }
         break; // eof TSK_HOME
       default: // unhandled task
         break; // eof default

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/directGPSNaviControl.c - Rev 2044 → 2045