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