Go to most recent revision | Details | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
1438 | - | 1 | /***************************************************************************** |
2 | * Copyright (C) 2010 seb@exse.net * |
||
3 | * * |
||
4 | * This program is free software; you can redistribute it and/or modify * |
||
5 | * it under the terms of the GNU General Public License as published by * |
||
6 | * the Free Software Foundation; either version 2 of the License. * |
||
7 | * * |
||
8 | * This program is distributed in the hope that it will be useful, * |
||
9 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * |
||
10 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * |
||
11 | * GNU General Public License for more details. * |
||
12 | * * |
||
13 | * You should have received a copy of the GNU General Public License * |
||
14 | * along with this program; if not, write to the * |
||
15 | * Free Software Foundation, Inc., * |
||
16 | * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * |
||
17 | * * |
||
18 | *****************************************************************************/ |
||
19 | |||
20 | #include <avr/io.h> |
||
21 | #include <inttypes.h> |
||
22 | #include <stdlib.h> |
||
23 | #include <avr/pgmspace.h> |
||
24 | #include <math.h> |
||
25 | |||
26 | #include "main.h" |
||
27 | #include "osd.h" |
||
28 | #include "lcd.h" |
||
29 | #include "timer.h" |
||
30 | #include "usart.h" |
||
31 | |||
32 | #include "mk-data-structs.h" |
||
33 | |||
34 | |||
35 | NaviData_t *naviData; |
||
36 | |||
37 | #define TIMEOUT 200 // 2 sec |
||
38 | |||
39 | |||
40 | void pwm (void) |
||
41 | { |
||
42 | |||
43 | if (hardware == FC) |
||
44 | { |
||
45 | lcd_printp_at(0, 3, PSTR("Only with NC !"), 0); |
||
46 | timer = 100; |
||
47 | while (timer > 0); |
||
48 | return; |
||
49 | } |
||
50 | |||
51 | lcd_cls(); |
||
52 | |||
53 | SwitchToNC(); |
||
54 | |||
55 | mode = 'O'; |
||
56 | |||
57 | // disable debug... |
||
58 | // RS232_request_mk_data (0, 'd', 0); |
||
59 | uint8_t tmp_dat; |
||
60 | tmp_dat = 0; |
||
61 | SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1); |
||
62 | |||
63 | // request OSD Data from NC every 100ms |
||
64 | // RS232_request_mk_data (1, 'o', 100); |
||
65 | tmp_dat = 10; |
||
66 | SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1); |
||
67 | |||
68 | timer = TIMEOUT; |
||
69 | abo_timer = ABO_TIMEOUT; |
||
70 | |||
71 | |||
72 | lcd_printp_at(0, 0, PSTR("GPS Alt:"), 0); |
||
73 | lcd_printp_at(0, 1, PSTR("Bar Alt:"), 0); |
||
74 | lcd_printp_at(0, 2, PSTR("Distance:"), 0); |
||
75 | lcd_printp_at(0, 3, PSTR("Bearing:"), 0); |
||
76 | lcd_printp_at(0, 4, PSTR("V-Angle:"), 0); |
||
77 | |||
78 | uint16_t bearing = 36; |
||
79 | |||
80 | do |
||
81 | { |
||
82 | if (rxd_buffer_locked) |
||
83 | { |
||
84 | timer = TIMEOUT; |
||
85 | Decode64 (); |
||
86 | naviData = (NaviData_t *) pRxData; |
||
87 | |||
88 | GPS_Pos_t currpos; |
||
89 | currpos.Latitude = naviData->CurrentPosition.Latitude; |
||
90 | currpos.Longitude = naviData->CurrentPosition.Longitude; |
||
91 | |||
92 | |||
93 | write_ndigit_number_u (11, 2, naviData->HomePositionDeviation.Distance / 10, 3, 0); |
||
94 | lcd_putc (14, 2, 'm', 0); |
||
95 | write_ndigit_number_u (11, 3, naviData->HomePositionDeviation.Bearing, 3, 0); |
||
96 | lcd_putc (14, 3, 'm', 0); |
||
97 | |||
98 | |||
99 | if (naviData->Altimeter > 300 || naviData->Altimeter < -300) |
||
100 | { |
||
101 | // above 10m only write full meters |
||
102 | write_ndigit_number_s (10, 1, naviData->Altimeter / 30, 4, 0); |
||
103 | } |
||
104 | else |
||
105 | { |
||
106 | // up to 10m write meters.dm |
||
107 | write_ndigit_number_s_10th (10, 1, naviData->Altimeter / 3, 3, 0); |
||
108 | } |
||
109 | lcd_putc (14, 1, 'm', 0); |
||
110 | |||
111 | |||
112 | |||
113 | |||
114 | if (((naviData->HomePosition.Altitude - naviData->CurrentPosition.Altitude)/1000) > 100 || ((naviData->HomePosition.Altitude - naviData->CurrentPosition.Altitude)/1000) < -100) |
||
115 | { |
||
116 | // above 10m only write full meters |
||
117 | write_ndigit_number_s (9, 0, (naviData->HomePosition.Altitude - naviData->CurrentPosition.Altitude) / 1000, 5, 0); |
||
118 | } |
||
119 | else |
||
120 | { |
||
121 | // up to 10m write meters.dm |
||
122 | write_ndigit_number_s_10th (9, 0, (naviData->HomePosition.Altitude - naviData->CurrentPosition.Altitude) / 100, 4, 0); |
||
123 | } |
||
124 | lcd_putc (14, 0, 'm', 0); |
||
125 | |||
126 | |||
127 | |||
128 | uint16_t deg = atan2(naviData->Altimeter / 3, naviData->HomePositionDeviation.Distance)*180/M_PI; |
||
129 | |||
130 | if(deg>90)deg=90; |
||
131 | if(deg<0)deg=0; |
||
132 | write_ndigit_number_s (11, 4, deg , 4, 0); |
||
133 | |||
134 | // wenn hoeher 10 order weiter weg als 10 |
||
135 | if((naviData->Altimeter > 300)||( naviData->HomePositionDeviation.Distance > 100)) |
||
136 | { |
||
137 | set_pwm_b(deg); |
||
138 | } |
||
139 | |||
140 | // wenn weiter weg als 20 |
||
141 | if ( naviData->HomePositionDeviation.Distance > 200) |
||
142 | { |
||
143 | //set_pwm_a(360-naviData->HomePositionDeviation.Bearing); |
||
144 | } |
||
145 | set_pwm_a(360-bearing); |
||
146 | |||
147 | |||
148 | write_ndigit_number_s (7, 7, bearing, 5, 0); |
||
149 | write_ndigit_number_s (0, 7, OCR1A, 5, 0); |
||
150 | |||
151 | write_ndigit_number_u (0, 6, naviData->UBat, 4, 0); |
||
152 | |||
153 | uint8_t cell = (naviData->UBat)/4; |
||
154 | |||
155 | if(naviData->UBat < 127) |
||
156 | cell = (naviData->UBat)/3; |
||
157 | |||
158 | write_ndigit_number_u (10, 6, cell, 4, 0); |
||
159 | |||
160 | |||
161 | |||
162 | lcd_frect ((8*0), (8*5), (cell-34)*16 , 6, 1); |
||
163 | |||
164 | rxd_buffer_locked = FALSE; |
||
165 | } |
||
166 | |||
167 | if (!abo_timer) |
||
168 | { // renew abo every 3 sec |
||
169 | // request OSD Data from NC every 100ms |
||
170 | // RS232_request_mk_data (1, 'o', 100); |
||
171 | tmp_dat = 10; |
||
172 | SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1); |
||
173 | |||
174 | abo_timer = ABO_TIMEOUT; |
||
175 | } |
||
176 | if (get_key_press (1 << KEY_PLUS)) |
||
177 | { |
||
178 | bearing+=36; |
||
179 | } |
||
180 | if (get_key_press (1 << KEY_MINUS)) |
||
181 | { |
||
182 | bearing-=36; |
||
183 | } |
||
184 | |||
185 | |||
186 | } |
||
187 | while (!get_key_press (1 << KEY_ESC) && timer); |
||
188 | |||
189 | tmp_dat = 0; |
||
190 | SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1); |
||
191 | |||
192 | mode = 0; |
||
193 | rxd_buffer_locked = FALSE; |
||
194 | |||
195 | if (!timer) |
||
196 | { // timeout occured |
||
197 | lcd_cls (); |
||
198 | |||
199 | lcd_printp_at (0, 0, PSTR("ERROR: no data"), 0); |
||
200 | |||
201 | timer = 100; |
||
202 | while (timer > 0); |
||
203 | pwm(); |
||
204 | } |
||
205 | } |