WebSVN - FlightCtrl - Diff - Rev 1612 and 1775 - /branches/dongfang_FC

 // +  POSSIBILITY OF SUCH DAMAGE.
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 #include <inttypes.h>
 #include <stdlib.h>
 #include "ubx.h"
-#include "mymath.h"
+//#include "mymath.h"
 #include "timer0.h"
 #include "uart0.h"
 #include "rc.h"
 #include "eeprom.h"
 // the current flight mode
 FlightMode_t FlightMode = GPS_FLIGHT_MODE_UNDEF;
-// ---------------------------------------------------------------------------------
-void GPS_UpdateParameter(void)
+// ---------------------------------------------------------------------------------
-{
+void GPS_UpdateParameter(void) {
   static FlightMode_t FlightModeOld = GPS_FLIGHT_MODE_UNDEF;
-  if((RC_Quality < 100) || (MKFlags & MKFLAG_EMERGENCY_LANDING))
-    {
-      FlightMode = GPS_FLIGHT_MODE_FREE;
+  if((RC_Quality < 100) || (MKFlags & MKFLAG_EMERGENCY_LANDING)) {
-    }
-  else
+    FlightMode = GPS_FLIGHT_MODE_FREE;
-    {
+  } else {
-      if     (dynamicParams.NaviGpsModeControl <  50) FlightMode = GPS_FLIGHT_MODE_AID;
+    if     (dynamicParams.NaviGpsModeControl <  50) FlightMode = GPS_FLIGHT_MODE_AID;
-      else if(dynamicParams.NaviGpsModeControl < 180) FlightMode = GPS_FLIGHT_MODE_FREE;
+    else if(dynamicParams.NaviGpsModeControl < 180) FlightMode = GPS_FLIGHT_MODE_FREE;
+    else                                          FlightMode = GPS_FLIGHT_MODE_HOME;
-      else                                                FlightMode = GPS_FLIGHT_MODE_HOME;
+  }
-    }
-  if (FlightMode != FlightModeOld)
-    {
+  if (FlightMode != FlightModeOld) {
-      BeepTime = 100;
+    BeepTime = 100;
+  }
-  FlightModeOld = FlightMode;
-}
   FlightModeOld = FlightMode;
+}
-// ---------------------------------------------------------------------------------
-// This function defines a good GPS signal condition
+// ---------------------------------------------------------------------------------
-uint8_t GPS_IsSignalOK(void)
+// This function defines a good GPS signal condition
-{
-  static uint8_t GPSFix = 0;
+uint8_t GPS_IsSignalOK(void) {
-  if( (GPSInfo.status != INVALID)  && (GPSInfo.satfix == SATFIX_3D) && (GPSInfo.flags & FLAG_GPSFIXOK) && ((GPSInfo.satnum >= staticParams.NaviGpsMinSat) || GPSFix))
+  static uint8_t GPSFix = 0;
-    {
-      GPSFix = 1;
+  if( (GPSInfo.status != INVALID)  && (GPSInfo.satfix == SATFIX_3D) && (GPSInfo.flags & FLAG_GPSFIXOK) && ((GPSInfo.satnum >= staticParams.NaviGpsMinSat) || GPSFix)) {
-      return(1);
+    GPSFix = 1;
     return 1;
+  }
   else return (0);
+}
-}
-// ---------------------------------------------------------------------------------
-// rescale xy-vector length to  limit
+// ---------------------------------------------------------------------------------
-uint8_t GPS_LimitXY(int32_t *x, int32_t *y, int32_t limit)
+// rescale xy-vector length to  limit
 uint8_t GPS_LimitXY(int32_t *x, int32_t *y, int32_t limit) {
   uint8_t retval = 0;
-  int32_t len;
-  len = (int32_t)c_sqrt(*x * *x + *y * *y);
+  int32_t len;
-  if (len > limit)
+  len = (int32_t)c_sqrt(*x * *x + *y * *y);
-    {
+  if (len > limit) {
-      // normalize control vector components to the limit
+    // normalize control vector components to the limit
-      *x  = (*x  * limit) / len;
+    *x  = (*x  * limit) / len;
-      *y  = (*y  * limit) / len;
+    *y  = (*y  * limit) / len;
-      retval = 1;
+    retval = 1;
+  }
   return(retval);
-}
+}
 // checks nick and roll sticks for manual control
-uint8_t GPS_IsManualControlled(void)
+// checks nick and roll sticks for manual control
 uint8_t GPS_IsManualControlled(void) {
-  if ( (abs(PPM_in[staticParams.ChannelAssignment[CH_NICK]]) < staticParams.NaviStickThreshold) && (abs(PPM_in[staticParams.ChannelAssignment[CH_ROLL]]) < staticParams.NaviStickThreshold)) return 0;
+  if ((abs(PPM_in[staticParams.ChannelAssignment[CH_NICK]]) < staticParams.NaviStickThreshold) && (abs(PPM_in[staticParams.ChannelAssignment[CH_ROLL]]) < staticParams.NaviStickThreshold)) return 0;
-  else return 1;
   else return 1;
+}
-// set given position to current gps position
-uint8_t GPS_SetCurrPosition(GPS_Pos_t * pGPSPos)
-{
+// set given position to current gps position
-  uint8_t retval = 0;
+uint8_t GPS_SetCurrPosition(GPS_Pos_t * pGPSPos) {
-  if(pGPSPos == NULL) return(retval);   // bad pointer
+  uint8_t retval = 0;
+  if(pGPSPos == NULL) return(retval);   // bad pointer
-  if(GPS_IsSignalOK())
-    {   // is GPS signal condition is fine
-      pGPSPos->Longitude        = GPSInfo.longitude;
-      pGPSPos->Latitude = GPSInfo.latitude;
+  if(GPS_IsSignalOK()) {        // is GPS signal condition is fine
-      pGPSPos->Altitude = GPSInfo.altitude;
+    pGPSPos->Longitude  = GPSInfo.longitude;
-      pGPSPos->Status   = NEWDATA;
+    pGPSPos->Latitude   = GPSInfo.latitude;
-      retval = 1;
+    pGPSPos->Altitude   = GPSInfo.altitude;
     pGPSPos->Status     = NEWDATA;
-  else
+    retval = 1;
-    {   // bad GPS signal condition
+  } else {      // bad GPS signal condition
-      pGPSPos->Status = INVALID;
+    pGPSPos->Status = INVALID;
-      retval = 0;
     retval = 0;
+  }
   return(retval);
 }
-// clear position
-uint8_t GPS_ClearPosition(GPS_Pos_t * pGPSPos)
+// clear position
-{
+uint8_t GPS_ClearPosition(GPS_Pos_t * pGPSPos) {
   uint8_t retval = 0;
-  if(pGPSPos == NULL) return(retval);   // bad pointer
+  if(pGPSPos == NULL)
-  else
+    return retval;      // bad pointer
   else {
-      pGPSPos->Longitude        = 0;
+    pGPSPos->Longitude  = 0;
-      pGPSPos->Latitude = 0;
+    pGPSPos->Latitude   = 0;
-      pGPSPos->Altitude = 0;
+    pGPSPos->Altitude   = 0;
-      pGPSPos->Status   = INVALID;
-      retval = 1;
+    pGPSPos->Status     = INVALID;
     retval = 1;
   return (retval);
   return (retval);
+}
 // disable GPS control sticks
-void GPS_Neutral(void)
+// disable GPS control sticks
-{
-  GPSStickNick = 0;
+void GPS_Neutral(void) {
-  GPSStickRoll = 0;
+  GPSStickNick = 0;
   GPSStickRoll = 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
+// calculates the GPS control stick values from the deviation to target position
-// then the P part of the controller is deactivated.
+// if the pointer to the target positin is NULL or is the target position invalid
-void GPS_PIDController(GPS_Pos_t *pTargetPos)
+// then the P part of the controller is deactivated.
 void GPS_PIDController(GPS_Pos_t *pTargetPos) {
-      PID_Nick =   (coscompass * PID_North + sincompass * PID_East) / 8192;
+    sincompass = (int32_t)c_sin_8192(YawGyroHeading / GYRO_DEG_FACTOR);
-      PID_Roll  =  (sincompass * PID_North - coscompass * PID_East) / 8192;
+    PID_Nick =   (coscompass * PID_North + sincompass * PID_East) / 8192;
     PID_Roll  =  (sincompass * PID_North - coscompass * PID_East) / 8192;
-      // limit resulting GPS control vector
+    // limit resulting GPS control vector
-      GPS_LimitXY(&PID_Nick, &PID_Roll, GPS_STICK_LIMIT);
+    GPS_LimitXY(&PID_Nick, &PID_Roll, GPS_STICK_LIMIT);
-      GPSStickNick = (int16_t)PID_Nick;
+    GPSStickNick = (int16_t)PID_Nick;
-      GPSStickRoll = (int16_t)PID_Roll;
-    }
-  else // invalid GPS data or bad compass reading
-    {
+    GPSStickRoll = (int16_t)PID_Roll;
-      GPS_Neutral(); // do nothing
       // reset error integral
-      GPSPosDevIntegral_North = 0;
+  else // invalid GPS data or bad compass reading
       GPSPosDevIntegral_East = 0;
       GPS_Neutral(); // do nothing
-}
+      // reset error integral
+      GPSPosDevIntegral_North = 0;
+      GPSPosDevIntegral_East = 0;
+    }
-void GPS_Main(void)
+}
-{
-  static uint8_t GPS_P_Delay = 0;
-  static uint16_t beep_rythm = 0;
+void GPS_Main(void) {
+  static uint8_t GPS_P_Delay = 0;
-  GPS_UpdateParameter();
+  static uint16_t beep_rythm = 0;
-  // store home position if start of flight flag is set
-  if(MKFlags & MKFLAG_CALIBRATE)
+  GPS_UpdateParameter();
-    {
-      if(GPS_SetCurrPosition(&HomePosition)) BeepTime = 700;
+  // store home position if start of flight flag is set
-    }
+  if(MKFlags & MKFLAG_CALIBRATE) {
+    if(GPS_SetCurrPosition(&HomePosition)) BeepTime = 700;
-  switch(GPSInfo.status)
+  }
-    {
-    case INVALID:  // invalid gps data
+  switch(GPSInfo.status) {
-      GPS_Neutral();
+  case INVALID:  // invalid gps data
-      if(FlightMode != GPS_FLIGHT_MODE_FREE)
+    GPS_Neutral();
-        {
+    if(FlightMode != GPS_FLIGHT_MODE_FREE) {
-          BeepTime = 100; // beep if signal is neccesary
+      BeepTime = 100; // beep if signal is neccesary
-        }
+    }
-      break;
+    break;
-    case PROCESSED: // if gps data are already processed do nothing
+  case PROCESSED: // if gps data are already processed do nothing
-      // downcount timeout
+    // downcount timeout
-      if(GPSTimeout) GPSTimeout--;
+    if(GPSTimeout) GPSTimeout--;
-      // if no new data arrived within timeout set current data invalid
+    // if no new data arrived within timeout set current data invalid
-      // and therefore disable GPS
+    // and therefore disable GPS
-      else
+    else
-        {
+      {
-          GPS_Neutral();
+        GPS_Neutral();
           GPSInfo.status = INVALID;
-        }
+      }
-      break;
+    break;
-    case NEWDATA: // new valid data from gps device
+  case NEWDATA: // new valid data from gps device
-      // if the gps data quality is good
+    // if the gps data quality is good
-      beep_rythm++;
+    beep_rythm++;
-      if (GPS_IsSignalOK())
+    if (GPS_IsSignalOK())
-        {
+      {
-          switch(FlightMode) // check what's to do
+        switch(FlightMode) // check what's to do
-            {
+          {
-            case GPS_FLIGHT_MODE_FREE:
+          case GPS_FLIGHT_MODE_FREE:
-              // update hold position to current gps position
+            // update hold position to current gps position
-              GPS_SetCurrPosition(&HoldPosition); // can get invalid if gps signal is bad
+            GPS_SetCurrPosition(&HoldPosition); // can get invalid if gps signal is bad
-              // disable gps control
+            // disable gps control
-              GPS_Neutral();
+            GPS_Neutral();
-              break;
+            break;
-            case GPS_FLIGHT_MODE_AID:
+          case GPS_FLIGHT_MODE_AID:
-              if(HoldPosition.Status != INVALID)
+            if(HoldPosition.Status != INVALID)
-                {
+              {
-                  if( GPS_IsManualControlled() ) // MK controlled by user
+                if( GPS_IsManualControlled() ) // MK controlled by user
-                    {
+                  {
-                      // update hold point to current gps position
+                    // update hold point to current gps position
-                      GPS_SetCurrPosition(&HoldPosition);
+                    GPS_SetCurrPosition(&HoldPosition);
-                      // disable gps control
+                    // disable gps control
-                      GPS_Neutral();
+                    GPS_Neutral();
-                      GPS_P_Delay = 0;
+                    GPS_P_Delay = 0;
-                    }
+                  }
-                  else // GPS control active
+                else // GPS control active
-                    {
+                  {
-                      if(GPS_P_Delay < 7)
+                    if(GPS_P_Delay < 7)
-                        { // delayed activation of P-Part for 8 cycles (8*0.25s = 2s)
+                      { // delayed activation of P-Part for 8 cycles (8*0.25s = 2s)
-                          GPS_P_Delay++;
+                        GPS_P_Delay++;
-                          GPS_SetCurrPosition(&HoldPosition); // update hold point to current gps position
+                        GPS_SetCurrPosition(&HoldPosition); // update hold point to current gps position
-                          GPS_PIDController(NULL); // activates only the D-Part
+                        GPS_PIDController(NULL); // activates only the D-Part
-                        }
+                      }
-                      else GPS_PIDController(&HoldPosition);// activates the P&D-Part
+                    else GPS_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
-                  GPS_SetCurrPosition(&HoldPosition);
+                GPS_SetCurrPosition(&HoldPosition);
-                  GPS_Neutral();
+                GPS_Neutral();
-                }
+              }
-              break;
+            break;
-            case GPS_FLIGHT_MODE_HOME:
+          case GPS_FLIGHT_MODE_HOME:
-              if(HomePosition.Status != INVALID)
+            if(HomePosition.Status != INVALID)
-                {
+              {
-                  // update hold point to current gps position
+                // update hold point to current gps position
-                  // to avoid a flight back if home comming is deactivated
+                // to avoid a flight back if home comming is deactivated
-                  GPS_SetCurrPosition(&HoldPosition);
+                GPS_SetCurrPosition(&HoldPosition);
-                  if( GPS_IsManualControlled() ) // MK controlled by user
+                if( GPS_IsManualControlled() ) // MK controlled by user
-                    {
+                  {
-                      GPS_Neutral();
+                    GPS_Neutral();
-                    }
+                  }
-                  else // GPS control active
+                else // GPS control active
-                    {
+                  {
-                      GPS_PIDController(&HomePosition);
+                    GPS_PIDController(&HomePosition);
-                    }
+                  }
-                }
+              }
-              else // bad home position
+            else // bad home position
-                {
+              {
-                  BeepTime = 50; // signal invalid home position
+                BeepTime = 50; // signal invalid home position
-                  // try to hold at least the position as a fallback option
+                // try to hold at least the position as a fallback option
-                  if (HoldPosition.Status != INVALID)
+                if (HoldPosition.Status != INVALID)
-                    {
+                  {
-                      if( GPS_IsManualControlled() ) // MK controlled by user
+                    if( GPS_IsManualControlled() ) // MK controlled by user
-                        {
+                      {
-                          GPS_Neutral();
+                        GPS_Neutral();
-                        }
+                      }
-                      else // GPS control active
+                    else // GPS control active
-                        {
+                      {
-                          GPS_PIDController(&HoldPosition);
+                        GPS_PIDController(&HoldPosition);
-                        }
+                      }
-                    }
+                  }
-                  else
+                else
-                    { // try to catch a valid hold position
+                  { // try to catch a valid hold position
-                      GPS_SetCurrPosition(&HoldPosition);
+                    GPS_SetCurrPosition(&HoldPosition);
-                      GPS_Neutral();
+                    GPS_Neutral();
-                    }
+                  }
-                }
+              }
-              break; // eof TSK_HOME
+            break; // eof TSK_HOME
-            default: // unhandled task
+          default: // unhandled task
-              GPS_Neutral();
+            GPS_Neutral();
-              break; // eof default
+            break; // eof default
-            } // eof switch GPS_Task
+          } // 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

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/gps.c - Rev 1612 → 1775