Subversion Repositories FlightCtrl

/*

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 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 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)

{

DisplayBuff[DispPtr++] = zeichen; return(1);

}

else if (PrintZiel == OUT_OSD)

{

OSDBuff[OSDPtr++] = zeichen; return(1);

}

else

{

return(uart_putchar(zeichen));

}

}

*/

/* ****************************************************************************

  Functionname:     OsdClear                      */ /*!

  Description:        

  @param[in]        

  @return           void

  @pre              -

  @post             -

  @author           Michael Walter

**************************************************************************** */

void OsdClear(void)

{

  unsigned char 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;

  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 */

  OSD_printf ("\33[2J");

  Delay_ms_Mess(300);

  OSD_printf ("\33[2J");

  /* Set OSD Pixel Clock */

  Delay_ms_Mess(300);

  OSD_printf ("\33[23;6v");

  /* Set OSD Pixel Width */

  Delay_ms_Mess(300);

  OSD_printf ("\33[25;448v");

  /* Set OSD Font */

  Delay_ms_Mess(300);

  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';

  if (UART2PrintAbgeschlossen == 1)

  {

    UART2PrintAbgeschlossen = 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;

  unsigned char tmp_tx;

  if(!UART2PrintAbgeschlossen)  

  {

    ptr++;   // die [0] wurde schon gesendet

    tmp_tx = OSDBuff[ptr];  

    if((tmp_tx == '\r')  /* tmp_tx == 0 */)

    {

      ptr = 0;

      UART2PrintAbgeschlossen = 1;

    }

    UDR1 = tmp_tx;

  }

  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 ---*/

/*--- VARIABLES ---*/

  static int dx, dy, x1,y1,x2,y2;

  f32_t sinPhi_P_cosTheta_P;

  static int x1_old, y1_old, x2_old, y2_old;

  static int iState = 0;

  switch (iState)

  {

  case 0:

    OSD_printf ("\33[0;11HHDG:%03d", status.iPsi10 / 10);

    iState++;

    break;

  case 1: /* Display the battery voltage an the RC signal level */

    OSD_printf ("\33[0;0HU:%03d\33[1;0HR:%03d", UBat, RCQuality);

    iState++;

    break;

  case 2: /* Display the current altitude */

    if (targetPosValid == 1)

    {

      int DeltaGPSAltitude = (actualPos.altitude - targetPos.altitude);

      OSD_printf ("\33[18;20HAGL:%03d\33[17;20HBar:%03d", DeltaGPSAltitude, CurrentAltitude);

    }

    else

    {

      OSD_printf ("\33[18;20HAGL:-.-\33[17;20HBar:%03d", CurrentAltitude);

    }

    iState++;

    break;

  case 3: /* 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 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++;

    break;

  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);

    iState++;

    break;

  case 11:

    OSD_printf ("\33[16;20HClb:%03d", DeltaAltitude);

    iState++;

    break;

  case 12:/*  Draw a ^ to indicate the target direction  */

    {

      static int LastGierOffset = 0;

      int GierOffset = (TargetGier - status.iPsi10) / 10;

      if (GierOffset > 180)

      {

        GierOffset -= 360;

      }

      if (GierOffset < -180)

      {

        GierOffset += 360;

      }

      GierOffset /= 14;

      OSD_printf ("\33[2;%dH \33[2;%dH^", 14 + LastGierOffset,14 + GierOffset);

      LastGierOffset = GierOffset;

      iState = 14;

      break;

    }

  case 13: /* Display the GPS_Nick and  GPS_Roll / StickNick and StickRoll */

    if (Override == 0)

    {

      OSD_printf ("\33[18;0HGN:%03d GR:%03d ", GPS_Nick, GPS_Roll);

    }

    else

    {

      OSD_printf ("\33[18;0HSN:%03d SR:%03d ", StickNick, StickRoll);

    }

    iState++;

    break;

  case 14:

#if 1/* Display the Horizon  */

    OSD_printf ("\33[156;120.r\33[204;120+r\33[/r ");

#endif       

    iState=0;

    break;

  default:

    iState = 0;

  }

}