1 | #include "define.h"
|
2 | #include "types.h"
|
3 | #include "global.h"
|
4 |
|
5 | #include "M_AVR_PROC/M_AVR_PROC.h"
|
6 | #include "M_STEER_MANAGEMENT/M_STEER_MANAGEMENT.h"
|
7 | #include "M_ROUTE_MANAGEMENT/M_ROUTE_MANAGEMENT.h"
|
8 | #include "M_LSM303DLH/M_LSM_303DLH.h"
|
9 | #include "M_GPS/M_GPS.h"
|
10 | #include "M_PROTOCOL/M_PROTOCOL.h"
|
11 | #include "M_PROTOCOL/fifo.h"
|
12 |
|
13 | #include <avr/io.h>
|
14 | #include <stdlib.h>
|
15 | #include <stdio.h>
|
16 | #include <avr/wdt.h>
|
17 |
|
18 |
|
19 | //FIFO
|
20 | fifo_t fifo_pc_in;
|
21 | fifo_t fifo_pc_out;
|
22 | fifo_t fifo_gps;
|
23 | uint8_t fifo_buffer_in[60];
|
24 | uint8_t fifo_buffer_out[60];
|
25 | uint8_t fifo_buffer_gps[60];
|
26 |
|
27 | //Timeouts
|
28 | #define TIMEOUT_GPS 1000
|
29 | #define TIMEOUT_WI232 1000
|
30 | #define TIMEOUT_COMPASS 250
|
31 | #define TIMEOUT_HEARTBEAT 500
|
32 | #define TIMEOUT_PPM 75
|
33 |
|
34 | //Timer Hardware
|
35 | unsigned long timer_gps_lc;
|
36 | unsigned long timer_wi232_lc;
|
37 | unsigned long timer_compass_lc;
|
38 |
|
39 | //Timer Software
|
40 | unsigned long timer_heartbeat_lc;
|
41 | unsigned long timer_ppm_lc;
|
42 |
|
43 | //Checks Send
|
44 | uint8_t timer_wi232_lc_send_check;
|
45 |
|
46 | int main(void)
|
47 | {
|
48 | fifo_init(&fifo_pc_in,fifo_buffer_in,60);
|
49 | fifo_init(&fifo_pc_out,fifo_buffer_out,60);
|
50 | fifo_init(&fifo_gps,fifo_buffer_gps,60);
|
51 |
|
52 | M_AVR_PROC_INIT();
|
53 | PORTB |= LED_RED|LED_GREEN;
|
54 |
|
55 | sei();
|
56 |
|
57 | delay_ms(20);
|
58 |
|
59 | PORTB &= ~ LED_GREEN;
|
60 |
|
61 | //Temporäre Kompass und Beschleunigungssensor Init
|
62 | uint8_t t = LSM303DLH_setup_acc();
|
63 | if(t!=M_LSM303DLH_ACC_OK)
|
64 | {
|
65 | device[E_T_DEVICE_COMPASS].valid = 0x00;
|
66 | uart_puts("ACC ERROR\n",M_USART_WI232);
|
67 | }
|
68 | else
|
69 | {
|
70 | device[E_T_DEVICE_COMPASS].valid = 0xff;
|
71 | }
|
72 |
|
73 | t = LSM303DLH_setup_mag();
|
74 | if(t!=M_LSM303DLH_MAG_OK)
|
75 | {
|
76 | device[E_T_DEVICE_COMPASS].valid = 0x00;
|
77 | uart_puts("MAG ERROR\n",M_USART_WI232);
|
78 | }
|
79 | else
|
80 | {
|
81 | device[E_T_DEVICE_COMPASS].valid = 0xff;
|
82 | }
|
83 |
|
84 |
|
85 | if( MCUSR &(1<<WDRF))
|
86 | {
|
87 | char a[10];
|
88 | uint8_t e = protocol_msg_code(PROTOCOL_RESET,PROTOCOL_WATCHDOG_RESET,a);
|
89 | for(uint8_t i=0;i<e;i++)
|
90 | {
|
91 | fifo_put(&fifo_pc_out,a[i]);
|
92 | UCSR0B |= (1<<UDRIE0);
|
93 | }
|
94 | }
|
95 | else
|
96 | {
|
97 | char a[10];
|
98 | uint8_t e = protocol_msg_code(PROTOCOL_RESET,PROTOCOL_NORMAL_RESET,a);
|
99 | for(uint8_t i=0;i<e;i++)
|
100 | {
|
101 | fifo_put(&fifo_pc_out,a[i]);
|
102 | UCSR0B |= (1<<UDRIE0);
|
103 | }
|
104 | }
|
105 |
|
106 | wdt_enable(WDTO_8S);
|
107 |
|
108 | PORTB &= ~ LED_RED;
|
109 |
|
110 | while(1)
|
111 | {
|
112 | wdt_reset();
|
113 | /************************************************************************
|
114 | FIFO
|
115 | ************************************************************************/
|
116 | int tmp = fifo_get_nowait(&fifo_gps);
|
117 | if(tmp!=-1)
|
118 | {
|
119 | if(nmea_decode(tmp)==NMEA_DECODED)
|
120 | {
|
121 | timer_gps_lc = get_millis();
|
122 | device[E_T_DEVICE_GPS].valid = 0xff;
|
123 | }
|
124 |
|
125 | }
|
126 |
|
127 | tmp = fifo_get_nowait(&fifo_pc_in);
|
128 | if(tmp!=-1)
|
129 | {
|
130 | if(protocol_decode(tmp)==PROTOCOL_DECODED)
|
131 | {
|
132 | timer_wi232_lc = get_millis();
|
133 | timer_wi232_lc_send_check = 0x00;
|
134 | device[E_T_DEVICE_PC].valid = 0xff;
|
135 | }
|
136 | }
|
137 | //wdt_reset();
|
138 | /************************************************************************
|
139 | TIMER
|
140 | ************************************************************************/
|
141 | //WI232
|
142 | if(device[E_T_DEVICE_PC].valid!=0x00)
|
143 | {
|
144 | if((get_millis()>(timer_wi232_lc+TIMEOUT_WI232))&&(timer_wi232_lc_send_check!=0x00))
|
145 | {
|
146 | char buf[10];
|
147 | uint8_t t = protocol_msg_check_byte(buf);
|
148 | for(uint8_t i=0;i<t;i++)
|
149 | {
|
150 | fifo_put(&fifo_pc_out,buf[i]);
|
151 | }
|
152 | UCSR0B |= (1<<UDRIE0);
|
153 |
|
154 | device[E_T_DEVICE_PC].valid = 0x00;
|
155 | }
|
156 | else if(get_millis()>(timer_wi232_lc+TIMEOUT_WI232))
|
157 | {
|
158 | char buf[10];
|
159 | uint8_t t = protocol_msg_check_byte(buf);
|
160 | for(uint8_t i=0;i<t;i++)
|
161 | {
|
162 | fifo_put(&fifo_pc_out,buf[i]);
|
163 | }
|
164 | UCSR0B |= (1<<UDRIE0);
|
165 |
|
166 | timer_wi232_lc = get_millis();
|
167 | timer_wi232_lc_send_check = 0xff;
|
168 | }
|
169 | }
|
170 | //wdt_reset();
|
171 | //GPS
|
172 | if(device[E_T_DEVICE_GPS].valid!=0x00)
|
173 | {
|
174 | if(get_millis()>(timer_gps_lc+TIMEOUT_GPS))
|
175 | {
|
176 | device[E_T_DEVICE_GPS].valid = 0x00;
|
177 | }
|
178 | }
|
179 | //wdt_reset();
|
180 | //COMPASS
|
181 | if(get_millis()>(timer_compass_lc+TIMEOUT_COMPASS))
|
182 | {
|
183 | LSM303DLH_read();
|
184 | compass_course[E_T_COMPASS_COURSE].v = LSM303DLH_heading_calculate();
|
185 | timer_compass_lc = get_millis();
|
186 | }
|
187 | //wdt_reset();
|
188 | //HEARTBEAT
|
189 | if(device[E_T_DEVICE_PC].valid!=0x00)
|
190 | {
|
191 | if(get_millis()>(timer_heartbeat_lc+TIMEOUT_HEARTBEAT))
|
192 | {
|
193 | char buf[40];
|
194 | uint8_t a = protocol_msg_heartbeat(buf);
|
195 | for(int i=0;i<a;i++)
|
196 | {
|
197 | fifo_put(&fifo_pc_out,buf[i]);
|
198 | }
|
199 | UCSR0B |= (1<<UDRIE0);
|
200 | timer_heartbeat_lc = get_millis();
|
201 | }
|
202 | }
|
203 | //wdt_reset();
|
204 | //PPM
|
205 | if(get_millis()>(timer_ppm_lc+TIMEOUT_PPM))
|
206 | {
|
207 | //steer_management_ppm_update();
|
208 | steer_management_autopilot();
|
209 | route_management_update();
|
210 | steer_management_update();
|
211 | steer_management_send_data();
|
212 | timer_ppm_lc = get_millis();
|
213 | }
|
214 |
|
215 |
|
216 | }
|
217 | }
|