Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 965 → Rev 966

/branches/KalmanFilter MikeW/Bob4_OSD.c
14,23 → 14,30
 
#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 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 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);
 
48,19 → 55,19
/* 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));
}
if(PrintZiel == OUT_LCD)
{
DisplayBuff[DispPtr++] = zeichen; return(1);
}
else if (PrintZiel == OUT_OSD)
{
OSDBuff[OSDPtr++] = zeichen; return(1);
}
else
{
return(uart_putchar(zeichen));
}
}
*/
 
 
77,8 → 84,8
**************************************************************************** */
void OsdClear(void)
{
unsigned char i;
for(i=0;i<80;i++) OSDBuff[i] = ' ';
unsigned char i;
for(i=0;i<80;i++) OSDBuff[i] = ' ';
}
 
/* ****************************************************************************
94,8 → 101,8
**************************************************************************** */
void LcdClear(void)
{
unsigned char i;
for(i=0;i<80;i++) DisplayBuff[i] = ' ';
unsigned char i;
for(i=0;i<80;i++) DisplayBuff[i] = ' ';
}
 
 
112,22 → 119,22
**************************************************************************** */
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");
/* 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");
}
 
 
143,12 → 150,12
**************************************************************************** */
void UART2Print()
{
OSDBuff[OSDPtr] = '\r';
if (UART2PrintAbgeschlossen == 1)
{
UART2PrintAbgeschlossen = 0;
UDR1 = OSDBuff[0];
}
OSDBuff[OSDPtr] = '\r';
if (UART2PrintAbgeschlossen == 1)
{
UART2PrintAbgeschlossen = 0;
UDR1 = OSDBuff[0];
}
}
 
/* ****************************************************************************
162,21 → 169,21
**************************************************************************** */
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;
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;
}
 
 
193,162 → 200,165
**************************************************************************** */
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 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++;
break;
case 7: /* 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 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;
/*--- (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 9: /* 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 10:
OSD_printf ("\33[16;20HClb:%03d", DeltaAltitude);
iState++;
break;
case 11:/* 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++;
break;
}
case 12: /* 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 13:
#if 0 /* Display the Horizon */
OSD_printf ("\33[156;120.r\33[204;120+r\33[/r ");
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;
}
iState=0;
break;
default:
iState = 0;
}
}