| 0,0 → 1,354 |
|---|
| /* |
| 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" |
| |
| 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; |
| |
| 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 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; |
| #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 "); |
| #endif |
| iState=0; |
| break; |
| default: |
| iState = 0; |
| } |
| } |
| |