WebSVN - FlightCtrl - Diff - Rev 839 and 966 - /branches/KalmanFilter MikeW/Bob4

 /*
 Copyright 2008, by Michael Walter
 All functions written by Michael Walter are free software and can be redistributed and/or modified under the terms of the GNU Lesser
 General Public License as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but
 WITHOUT ANY WARRANTY, without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public
 License along with this program. If not, see <http://www.gnu.org/licenses/>.
 Please note: The software is based on the framework provided by H. Buss and I. Busker in their Mikrokopter projekt. All functions that
 are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de
 unless it is stated otherwise.
 */
 #include "main.h"
 #include "kafi.h"
+#include "mymath.h"
+#define sin45 -0.707106
+#define cos45 0.707106
 extern void UART2Print(void);
 void InitOSD(void);
 void SendOSD(void);
-extern  gpsInfo_t       actualPos;                      // measured position (last gps record)
+extern  gpsInfo_t actualPos;// measured position (last gps record)
-extern  gpsInfo_t       targetPos;                      // measured position (last gps record)
+extern  gpsInfo_t targetPos;// measured position (last gps record)
 extern signed int GPS_Nick;
 extern signed int GPS_Roll;
 extern int CurrentAltitude,  LastAltitude, targetPosValid, RCQuality;
 extern int nick_gain_p, nick_gain_d, roll_gain_p, roll_gain_d;
 extern int Override, TargetGier, DeltaAltitude, Theta45, Phi45;
-extern  f32_t sinPhi_P,  cosPhi_P, sinTheta_P, cosTheta_P;
+extern f32_t sinPhi_P,  cosPhi_P, sinTheta_P, cosTheta_P;
 extern unsigned long maxDistance;
+int Theta45;
+int Phi45;
 unsigned char UART2PrintAbgeschlossen = 1;
 void UART2Print(void);
 char DisplayBuff[80] = "";
 unsigned char DispPtr = 0;
 char OSDBuff[80] = "";
 unsigned char OSDPtr = 0;
 #define OSD_printf(format, args...)  { OSDPtr = 0;  _printf_P(OUT_OSD,PSTR(format) , ## args);UART2Print();}
 #define OSD_printf_(format, args...)  { _printf_P(OUT_OSD,PSTR(format) , ## args);}
 /* For OSD_printf to work, Putchar() has to be extendet as follows.
 char Putchar(char zeichen)
+{
-  if(PrintZiel == OUT_LCD)
+if(PrintZiel == OUT_LCD)
-  {
+{
-        DisplayBuff[DispPtr++] = zeichen; return(1);
+DisplayBuff[DispPtr++] = zeichen; return(1);
-  }
+}
-  else if (PrintZiel == OUT_OSD)
+else if (PrintZiel == OUT_OSD)
-  {
+{
-        OSDBuff[OSDPtr++] = zeichen; return(1);
+OSDBuff[OSDPtr++] = zeichen; return(1);
-  }
+}
-  else
+else
-  {
+{
-        return(uart_putchar(zeichen));
+return(uart_putchar(zeichen));
-  }
+}
+}
 */
 /* ****************************************************************************
   Functionname:     OsdClear                      */ /*!
   Description:
   @param[in]
   @return           void
   @pre              -
   @post             -
   @author           Michael Walter
 **************************************************************************** */
 void OsdClear(void)
+{
-        unsigned char i;
+  unsigned char i;
-        for(i=0;i<80;i++) OSDBuff[i] = ' ';
+  for(i=0;i<80;i++) OSDBuff[i] = ' ';
+}
 /* ****************************************************************************
   Functionname:     LcdClear                      */ /*!
   Description:
   @param[in]
   @return           void
   @pre              -
   @post             -
   @author
 **************************************************************************** */
 void LcdClear(void)
+{
-        unsigned char i;
+  unsigned char i;
-        for(i=0;i<80;i++) DisplayBuff[i] = ' ';
+  for(i=0;i<80;i++) DisplayBuff[i] = ' ';
+}
 /* ****************************************************************************
   Functionname:     InitOSD                      */ /*!
   Description:         Init the Bob-4H OSD
   @param[in]
   @return           void
   @pre              -
   @post             -
   @author           Michael Walter
 **************************************************************************** */
 void InitOSD()
+{
-        /* Clear OSD */
+  /* Clear OSD */
-        OSD_printf ("\33[2J");
+  OSD_printf ("\33[2J");
-        Delay_ms_Mess(300);
+  Delay_ms_Mess(300);
-        OSD_printf ("\33[2J");
+  OSD_printf ("\33[2J");
-    /* Set OSD Pixel Clock */
+  /* Set OSD Pixel Clock */
-        Delay_ms_Mess(300);
+  Delay_ms_Mess(300);
-        OSD_printf ("\33[23;6v");
+  OSD_printf ("\33[23;6v");
-        /* Set OSD Pixel Width */
+  /* Set OSD Pixel Width */
-        Delay_ms_Mess(300);
+  Delay_ms_Mess(300);
-        OSD_printf ("\33[25;448v");
+  OSD_printf ("\33[25;448v");
-    /* Set OSD Font */
+  /* Set OSD Font */
-        Delay_ms_Mess(300);
+  Delay_ms_Mess(300);
-        OSD_printf ("\33[0z");
+  OSD_printf ("\33[0z");
+}
 /* ****************************************************************************
 Functionname:     UART2Print                      */ /*!
 Description:          Send OSD Data to the Bob-4H OSD
   @return           void
   @pre              -
   @post             -
   @author           Michael Walter
 **************************************************************************** */
 void UART2Print()
+{
-        OSDBuff[OSDPtr] = '\r';
+  OSDBuff[OSDPtr] = '\r';
-        if (UART2PrintAbgeschlossen == 1)
+  if (UART2PrintAbgeschlossen == 1)
-        {
+  {
-                UART2PrintAbgeschlossen = 0;
+    UART2PrintAbgeschlossen = 0;
-                UDR1 = OSDBuff[0];
+    UDR1 = OSDBuff[0];
-        }
+  }
+}
 /* ****************************************************************************
 Functionname:     SIGNAL                      */ /*!
 Description:          Send OSD Data to the Bob-4H OSD
   @return           void
   @pre              -
   @post             -
   @author           Michael Walter
 **************************************************************************** */
 SIGNAL(SIG_USART1_TRANS)
+{
-        static unsigned int ptr = 0;
+  static unsigned int ptr = 0;
-        unsigned char tmp_tx;
+  unsigned char tmp_tx;
-        if(!UART2PrintAbgeschlossen)
+  if(!UART2PrintAbgeschlossen)
-        {
+  {
-                ptr++;   // die [0] wurde schon gesendet
+    ptr++;   // die [0] wurde schon gesendet
-                tmp_tx = OSDBuff[ptr];
+    tmp_tx = OSDBuff[ptr];
-                if((tmp_tx == '\r')  /* tmp_tx == 0 */)
+    if((tmp_tx == '\r')  /* tmp_tx == 0 */)
-                {
+    {
-                        ptr = 0;
+      ptr = 0;
-                        UART2PrintAbgeschlossen = 1;
+      UART2PrintAbgeschlossen = 1;
-                }
+    }
-                UDR1 = tmp_tx;
+    UDR1 = tmp_tx;
-        }
+  }
-        else ptr = 0;
+  else ptr = 0;
+}
 /* ****************************************************************************
   Functionname:     SendOSD                      */ /*!
   Description:          Display OnScreen Display Data on a Bob-4H
   @param[in]
   @return           void
   @pre              -
   @post             -
   @author           Michael Walter
 **************************************************************************** */
 void SendOSD()
+{
-        /*--- (SYMBOLIC) CONSTANTS ---*/
+/*--- (SYMBOLIC) CONSTANTS ---*/
-        /*--- VARIABLES ---*/
+/*--- VARIABLES ---*/
-        //static int dx, dy, x1,y1,x2,y2;
+  static int dx, dy, x1,y1,x2,y2;
-        //f32_t sinPhi_P_cosTheta_P;
+  f32_t sinPhi_P_cosTheta_P;
-        //static int x1_old, y1_old, x2_old, y2_old;
+  static int x1_old, y1_old, x2_old, y2_old;
-        static int iState = 0;
+  static int iState = 0;
-        switch (iState)
+  switch (iState)
-        {
+  {
-        case 0:
+  case 0:
-                OSD_printf ("\33[0;11HHDG:%03d", status.iPsi10 / 10);
+    OSD_printf ("\33[0;11HHDG:%03d", status.iPsi10 / 10);
-                iState++;
+    iState++;
-                break;
+    break;
-        case 1: /* Display the battery voltage an the RC signal level */
+  case 1: /* Display the battery voltage an the RC signal level */
-                OSD_printf ("\33[0;0HU:%03d\33[1;0HR:%03d", UBat, RCQuality);
+    OSD_printf ("\33[0;0HU:%03d\33[1;0HR:%03d", UBat, RCQuality);
-                iState++;
+    iState++;
-                break;
+    break;
-        case 2: /* Display the current altitude */
+  case 2: /* Display the current altitude */
-                if (targetPosValid == 1)
+    if (targetPosValid == 1)
-                {
+    {
-                        int DeltaGPSAltitude = (actualPos.altitude - targetPos.altitude);
+      int DeltaGPSAltitude = (actualPos.altitude - targetPos.altitude);
-                        OSD_printf ("\33[18;20HAGL:%03d\33[17;20HBar:%03d", DeltaGPSAltitude, CurrentAltitude);
+      OSD_printf ("\33[18;20HAGL:%03d\33[17;20HBar:%03d", DeltaGPSAltitude, CurrentAltitude);
-                }
+    }
-                else
+    else
-                {
+    {
-                        OSD_printf ("\33[18;20HAGL:-.-\33[17;20HBar:%03d", CurrentAltitude);
+      OSD_printf ("\33[18;20HAGL:-.-\33[17;20HBar:%03d", CurrentAltitude);
-                }
-                iState++;
-                break;
-        case 3: /* Draw an artificial horizon. Part 1 */
-#if 0
-                sinPhi_P_cosTheta_P =  sinPhi_P * cosTheta_P;
-                Theta45 = c_asin_8192((int)(sin45 * (-sinTheta_P + sinPhi_P_cosTheta_P ) * 8192.F));
-                Phi45 = c_atan2((int)(100.F * sin45 * (-sinTheta_P - sinPhi_P_cosTheta_P) / (cosPhi_P * cosTheta_P)) , 100);
-                dx = c_cos_8192(Phi45) / 128;
-                dy = c_sin_8192(Phi45) / 128;
-                x1 = 180 - dx;
-                y1 = 120 + 2 * Theta45 + dy;
-                x2 = 180 + dx;
-                y2 = y1 - 2 * dy;
-                OSDPtr = 0;
-                OSD_printf_("\33[%d;%d.r\33[%d;%d+r\33[0/r",x1_old,y1_old,x2_old,y2_old);
-#endif
-                iState++;
-                break;
-        case 4:  /* Draw an artificial horizon. Part 2 */
-#if 0
+    }
-                OSD_printf_("\33[%d;%d.r\33[%d;%d+r\33[/r",x1,y1,x2,y2);
-                UART2Print();
-                x1_old = x1;
-                y1_old = y1;
-                x2_old = x2;
-                y2_old = y2;
-#endif
-                iState++;
-                break;
-        case 5:  /* Display velocity over ground */
-                if (actualPos.state > 1)
-                {
-                        OSD_printf ("\33[0;20HVel:%03d", ((actualPos.groundSpeed / 10) * 36) /100);
-                }
-                else
-                {
-                        OSD_printf ("\33[0;20HVel:-.-");
-                }
-                iState++;
-                break;
-        case 6:  /* Display distance from target */
-                if (targetPosValid == 1)
-                {
-                        OSD_printf ("\33[1;20HDst:%03d", maxDistance / 10);
-                }
-                else
-                {
-                        OSD_printf ("\33[1;20HDst:-.-");
-                }
-                iState++;
+    iState++;
-                break;
+    break;
-        case 7: /* Draw an artificial horizon. Part 1 */
+  case 3: /* Draw an artificial horizon. Part 1 */
-#if 0
+#if 1
-                sinPhi_P_cosTheta_P =  sinPhi_P * cosTheta_P;
+    sinPhi_P_cosTheta_P =  sinPhi_P * cosTheta_P;
-                Theta45 = c_asin_8192((int)(sin45 * (-sinTheta_P + sinPhi_P_cosTheta_P ) * 8192.F));
+    Theta45 = c_asin_8192((int)(sin45 * (-sinTheta_P + sinPhi_P_cosTheta_P ) * 8192.F));
-                Phi45 = c_atan2((int)(100.F * sin45 * (-sinTheta_P - sinPhi_P_cosTheta_P) / (cosPhi_P * cosTheta_P)) , 100);
+    Phi45 = c_atan2((int)(100.F * sin45 * (-sinTheta_P - sinPhi_P_cosTheta_P) / (cosPhi_P * cosTheta_P)) , 100);
-                dx = c_cos_8192(Phi45) / 128;
+    dx = c_cos_8192(Phi45) / 128;
-                dy = c_sin_8192(Phi45) / 128;
+    dy = c_sin_8192(Phi45) / 128;
-                x1 = 180 - dx;
+    x1 = 180 - dx;
-                y1 = 120 + 2 * Theta45 + dy;
+    y1 = 120 + 2 * Theta45 + dy;
-                x2 = 180 + dx;
+    x2 = 180 + dx;
-                y2 = y1 - 2 * dy;
+    y2 = y1 - 2 * dy;
-                OSDPtr = 0;
+    OSDPtr = 0;
-                OSD_printf_("\33[%d;%d.r\33[%d;%d+r\33[0/r",x1_old,y1_old,x2_old,y2_old);
+    OSD_printf_("\33[%d;%d.r\33[%d;%d+r\33[0/r",x1_old,y1_old,x2_old,y2_old);
-#endif
-                iState++;
-                break;
-        case 8: /* Draw an artificial horizon. Part 2 */
-#if 0
-                OSD_printf_("\33[%d;%d.r\33[%d;%d+r\33[/r",x1,y1,x2,y2);
-                UART2Print();
-                x1_old = x1;
-                y1_old = y1;
-                x2_old = x2;
-                y2_old = y2;
 #endif
+    iState++;
+    break;
+  case 4:  /* Draw an artificial horizon. Part 2 */
+#if 1
+    OSD_printf_("\33[%d;%d.r\33[%d;%d+r\33[/r",x1,y1,x2,y2);
+    UART2Print();
+    x1_old = x1;
+    y1_old = y1;
+    x2_old = x2;
+    y2_old = y2;
+#endif
+    iState++;
+    break;
+  case 5:  /* Display velocity over ground */
+    if (actualPos.state > 1)
+    {
+      OSD_printf ("\33[0;20HVel:%03d", ((actualPos.groundSpeed / 10) * 36) /100);
+    }
+    else
+    {
+      OSD_printf ("\33[0;20HVel:-.-");
+    }
+    iState++;
+    break;
+  case 6:  /* Display distance from target */
+    if (targetPosValid == 1)
+    {
+      OSD_printf ("\33[1;20HDst:%03d", maxDistance / 10);
+    }
+    else
+    {
+      OSD_printf ("\33[1;20HDst:-.-");
+    }
+    iState = 8;
+    break;
+  case 7:
+    OSD_printf ("\33[4;0HN:%03d\33[5;0HR:%03d", status.iTheta10 / 10, status.iPhi10 / 10);
+    iState++;
+    break;
+  case 8: /* Draw an artificial horizon. Part 1 */
+#if 1
+    sinPhi_P_cosTheta_P =  sinPhi_P * cosTheta_P;
+    Theta45 = c_asin_8192((int)(sin45 * (-sinTheta_P + sinPhi_P_cosTheta_P ) * 8192.F));
+    Phi45 = c_atan2((int)(100.F * sin45 * (-sinTheta_P - sinPhi_P_cosTheta_P) / (cosPhi_P * cosTheta_P)) , 100);
+    dx = c_cos_8192(Phi45) / 128;
+    dy = c_sin_8192(Phi45) / 128;
+    x1 = 180 - dx;
+    y1 = 120 + 2 * Theta45 + dy;
+    x2 = 180 + dx;
+    y2 = y1 - 2 * dy;
+    OSDPtr = 0;
+    OSD_printf_("\33[%d;%d.r\33[%d;%d+r\33[0/r",x1_old,y1_old,x2_old,y2_old);
+#endif
+    iState++;
+    break;
+  case 9: /* Draw an artificial horizon. Part 2 */
+#if 1
+    OSD_printf_("\33[%d;%d.r\33[%d;%d+r\33[/r",x1,y1,x2,y2);
+    UART2Print();
+    x1_old = x1;
+    y1_old = y1;
+    x2_old = x2;
+    y2_old = y2;
+#endif
-                iState++;
+    iState++;
-                break;
+    break;
-        case 9: /* Display the GPS control outputs */
+  case 10: /* Display the GPS control outputs */
-                OSD_printf ("\33[16;0HNG:%03d EG:%03d \33[17;0HNV:%03d EV:%03d ", nick_gain_p,  roll_gain_p, nick_gain_d,  roll_gain_d);
+    OSD_printf ("\33[16;0HNG:%03d EG:%03d \33[17;0HNV:%03d EV:%03d ", nick_gain_p,  roll_gain_p, nick_gain_d,  roll_gain_d);
-                iState++;
+    iState++;
-                break;
+    break;
-        case 10:
+  case 11:
-                OSD_printf ("\33[16;20HClb:%03d", DeltaAltitude);
+    OSD_printf ("\33[16;20HClb:%03d", DeltaAltitude);
-                iState++;
+    iState++;
-                break;
+    break;
-        case 11:/*  Draw a ^ to indicate the target direction  */
+  case 12:/*  Draw a ^ to indicate the target direction  */
-                {
+    {
-                        static int LastGierOffset = 0;
+      static int LastGierOffset = 0;
-                        int GierOffset = (TargetGier - status.iPsi10) / 10;
+      int GierOffset = (TargetGier - status.iPsi10) / 10;
-                        if (GierOffset > 180)
+      if (GierOffset > 180)
-                        {
+      {
-                                GierOffset -= 360;
+        GierOffset -= 360;
-                        }
+      }
-                        if (GierOffset < -180)
+      if (GierOffset < -180)
-                        {
+      {
-                                GierOffset += 360;
+        GierOffset += 360;
-                        }
+      }
-                        GierOffset /= 14;
+      GierOffset /= 14;
-                        OSD_printf ("\33[2;%dH \33[2;%dH^", 14 + LastGierOffset,14 + GierOffset);
+      OSD_printf ("\33[2;%dH \33[2;%dH^", 14 + LastGierOffset,14 + GierOffset);
-                        LastGierOffset = GierOffset;
+      LastGierOffset = GierOffset;
-                        iState++;
+      iState = 14;
-                        break;
+      break;
-                }
+    }
-        case 12:         /* Display the GPS_Nick and  GPS_Roll / StickNick and StickRoll */
+  case 13: /* Display the GPS_Nick and  GPS_Roll / StickNick and StickRoll */
-                if (Override == 0)
+    if (Override == 0)
-                {
+    {
-                        OSD_printf ("\33[18;0HGN:%03d GR:%03d ", GPS_Nick, GPS_Roll);
+      OSD_printf ("\33[18;0HGN:%03d GR:%03d ", GPS_Nick, GPS_Roll);
-                }
+    }
-                else
+    else
-                {
+    {
-                        OSD_printf ("\33[18;0HSN:%03d SR:%03d ", StickNick, StickRoll);
+      OSD_printf ("\33[18;0HSN:%03d SR:%03d ", StickNick, StickRoll);
-                }
+    }
-                iState++;
+    iState++;
-                break;
+    break;
-        case 13:
+  case 14:
-#if 0           /* Display the Horizon  */
+#if 1/* Display the Horizon  */
-                OSD_printf ("\33[156;120.r\33[204;120+r\33[/r ");
+    OSD_printf ("\33[156;120.r\33[204;120+r\33[/r ");
 #endif
-                iState=0;
+    iState=0;
-                break;
+    break;
-        default:
+  default:
-                iState = 0;
+    iState = 0;
-                }
+  }
+}

Subversion Repositories FlightCtrl

(root)/branches/KalmanFilter MikeW/Bob4_OSD.c @ 1891 - Rev 839 → 966