Rev 839 |
Blame |
Compare with Previous |
Last modification |
View Log
| RSS feed
/*
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;
}
}