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;
   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;
+  uint8_t retval = 0;
-  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);
   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;
-  else return 1;
+  if ((abs(PPM_in[staticParams.ChannelAssignment[CH_NICK]]) < staticParams.NaviStickThreshold) && (abs(PPM_in[staticParams.ChannelAssignment[CH_ROLL]]) < staticParams.NaviStickThreshold)) return 0;
   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;
-      retval = 0;
+    pGPSPos->Status = INVALID;
+    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->Status   = INVALID;
+    pGPSPos->Altitude   = 0;
-      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) {
-  static int32_t PID_Nick, PID_Roll;
-  int32_t coscompass, sincompass;
-  int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm
+  static int32_t PID_Nick, PID_Roll;
-  int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, I_East = 0;
+  int32_t coscompass, sincompass;
-  int32_t PID_North = 0, PID_East = 0;
+  int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm
-  static int32_t cos_target_latitude = 1;
+  int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, I_East = 0;
-  static int32_t GPSPosDevIntegral_North = 0, GPSPosDevIntegral_East = 0;
+  int32_t PID_North = 0, PID_East = 0;
-      // I-Part
+    // I-Part
-      I_North = ((int32_t)dynamicParams.NaviGpsI * GPSPosDevIntegral_North)/8192;
+    I_North = ((int32_t)dynamicParams.NaviGpsI * GPSPosDevIntegral_North)/8192;
-      I_East =  ((int32_t)dynamicParams.NaviGpsI * GPSPosDevIntegral_East)/8192;
     I_East =  ((int32_t)dynamicParams.NaviGpsI * GPSPosDevIntegral_East)/8192;
-      // 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;
-      // combine PI- and D-Part
+    // combine PI- and D-Part
-      PID_North += D_North;
+    PID_North += D_North;
-      PID_East  += D_East;
     PID_East  += D_East;
-      // scale combination with gain.
+    // scale combination with gain.
-      PID_North = (PID_North * (int32_t)dynamicParams.NaviGpsGain) / 100;
+    PID_North = (PID_North * (int32_t)dynamicParams.NaviGpsGain) / 100;
-      PID_East  = (PID_East  * (int32_t)dynamicParams.NaviGpsGain) / 100;
     PID_East  = (PID_East  * (int32_t)dynamicParams.NaviGpsGain) / 100;
       // GPS to nick and roll settings
     // GPS to nick and roll settings
-      // A positive nick angle moves head downwards (flying forward).
+    // A positive nick angle moves head downwards (flying forward).
-      // A positive roll angle tilts left side downwards (flying left).
+    // A positive roll angle tilts left side downwards (flying left).
-      coscompass = (int32_t)c_cos_8192(YawGyroHeading / GYRO_DEG_FACTOR);
+    coscompass = (int32_t)c_cos_8192(YawGyroHeading / GYRO_DEG_FACTOR);
-      sincompass = (int32_t)c_sin_8192(YawGyroHeading / GYRO_DEG_FACTOR);
+    sincompass = (int32_t)c_sin_8192(YawGyroHeading / GYRO_DEG_FACTOR);
-      PID_Nick =   (coscompass * PID_North + sincompass * PID_East) / 8192;
+    PID_Nick =   (coscompass * PID_North + sincompass * PID_East) / 8192;
-      PID_Roll  =  (sincompass * PID_North - coscompass * 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);
       GPSPosDevIntegral_North = 0;
       GPSPosDevIntegral_East = 0;
+    }
+}
-void GPS_Main(void)
 void GPS_Main(void) {
   static uint8_t GPS_P_Delay = 0;
   static uint16_t beep_rythm = 0;
   GPS_UpdateParameter();
   // store home position if start of flight flag is set
-  if(MKFlags & MKFLAG_CALIBRATE)
+  // store home position if start of flight flag is set
-    {
+  if(MKFlags & MKFLAG_CALIBRATE) {
-      if(GPS_SetCurrPosition(&HomePosition)) BeepTime = 700;
     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

Subversion Repositories FlightCtrl

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