Details | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
1704 | - | 1 | #include <avr/boot.h> |
2 | |||
3 | #include <avr/io.h> |
||
4 | #include <avr/interrupt.h> |
||
5 | |||
6 | #include "main.h" |
||
7 | #include "timer0.h" |
||
8 | #include "uart0.h" |
||
9 | #include "uart1.h" |
||
10 | #include "fat16.h" |
||
11 | #include "led.h" |
||
12 | #include "menu.h" |
||
13 | #include "printf_P.h" |
||
14 | #include "analog.h" |
||
15 | #include "gps.h" |
||
16 | #include "button.h" |
||
17 | #include "logging.h" |
||
18 | #include "settings.h" |
||
19 | |||
20 | #ifdef USE_FOLLOWME |
||
21 | int16_t UBat = 120; |
||
22 | int16_t Zellenzahl = 0; |
||
23 | int16_t PowerOn = 0; |
||
24 | int16_t i = 0; |
||
25 | int16_t delay = 0; |
||
26 | uint16_t FOLLOWME_INTERVAL = 500; |
||
27 | uint8_t CELLUNDERVOLTAGE = 33; |
||
28 | uint8_t ToleranceRadius = 3; |
||
29 | uint8_t FollowMeSpeed = 20; |
||
30 | #endif |
||
31 | |||
32 | uint16_t Error = 0; |
||
33 | SysState_t SysState = STATE_UNDEFINED; |
||
34 | |||
35 | int main (void) |
||
36 | { |
||
37 | static uint16_t FollowMe_Timer = 0; |
||
38 | |||
39 | // disable interrupts global |
||
40 | //cli(); |
||
41 | |||
42 | // disable watchdog |
||
43 | MCUSR &=~(1<<WDRF); |
||
44 | WDTCSR |= (1<<WDCE)|(1<<WDE); |
||
45 | WDTCSR = 0; |
||
46 | |||
47 | // initalize modules |
||
48 | LED_Init(); |
||
49 | LEDRED_ON; |
||
50 | TIMER0_Init(); |
||
51 | USART0_Init(); |
||
52 | UBX_Init(); |
||
53 | USART1_Init(); |
||
54 | ADC_Init(); |
||
55 | Button_Init(); |
||
56 | // enable interrupts global |
||
57 | sei(); |
||
58 | |||
59 | // try to initialize the FAT 16 filesystem on the SD-Card |
||
60 | Fat16_Init(); |
||
61 | // initialize the settings |
||
62 | Settings_Init(); |
||
63 | // initialize logging (needs settings) |
||
64 | Logging_Init(); |
||
65 | |||
66 | LEDRED_OFF; |
||
67 | LEDGRN_ON; |
||
68 | |||
69 | //set Settings from settings.ini file |
||
70 | Settings_GetParamValue(PID_FOME_INTERV, &(FOLLOWME_INTERVAL)); |
||
71 | Settings_GetParamValue(PID_CELLUNDERVO, &(CELLUNDERVOLTAGE)); |
||
72 | Settings_GetParamValue(PID_TOLERANCERA, &(ToleranceRadius)); |
||
73 | Settings_GetParamValue(PID_FOLLOWSPEED, &(FollowMeSpeed)); |
||
74 | |||
75 | #ifdef USE_SDLOGGER |
||
76 | printf("\r\n\r\nHW: SD-Logger"); |
||
77 | #endif |
||
78 | #ifdef USE_FOLLOWME |
||
79 | printf("\r\n\r\nHW: Follow-Me"); |
||
80 | #endif |
||
81 | printf("\r\nFollow Me\r\nSoftware:V%d.%d%c ",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a'); |
||
82 | printf("\r\n------------------------------"); |
||
83 | printf("\r\n"); |
||
84 | |||
85 | |||
86 | BeepTime = 1000; |
||
87 | |||
88 | Menu_Clear(); |
||
89 | |||
90 | FollowMe_Timer = SetDelay(FOLLOWME_INTERVAL); |
||
91 | |||
92 | while (1) |
||
93 | { |
||
94 | // get gps data to update the follow me position |
||
95 | GPS_Update(); |
||
96 | DebugOut.Analog[10] = GPSData.NumOfSats; |
||
97 | |||
98 | // update logging |
||
99 | Logging_Update(); |
||
100 | |||
101 | // check for button action and change state resectively |
||
102 | if(GetButton()) |
||
103 | { |
||
104 | BeepTime = 200; |
||
105 | |||
106 | switch(SysState) |
||
107 | { |
||
108 | case STATE_IDLE: |
||
109 | if(!Error) |
||
110 | { |
||
111 | SysState = STATE_SEND_FOLLOWME; // activate followme only of no error has occured |
||
112 | } |
||
113 | break; |
||
114 | |||
115 | case STATE_SEND_FOLLOWME: |
||
116 | SysState = STATE_IDLE; |
||
117 | break; |
||
118 | default: |
||
119 | SysState = STATE_IDLE; |
||
120 | break; |
||
121 | } |
||
122 | |||
123 | } |
||
124 | |||
125 | // state machine |
||
126 | DebugOut.Analog[9] = SysState; |
||
127 | switch(SysState) |
||
128 | { |
||
129 | case(STATE_SEND_FOLLOWME): |
||
130 | { |
||
131 | if(CheckDelay(FollowMe_Timer)) // time for next message? |
||
132 | { |
||
133 | if(FollowMe.Position.Status == NEWDATA) // if new |
||
134 | { // update remaining data |
||
135 | FollowMe_Timer = SetDelay(FOLLOWME_INTERVAL); // reset timer |
||
136 | |||
137 | FollowMe.Heading = -1; // invalid heading |
||
138 | FollowMe.ToleranceRadius = ToleranceRadius; // 5 meter default |
||
139 | FollowMe.HoldTime = 30; // go home after 60s without any update |
||
140 | FollowMe.Event_Flag = 0; // no event |
||
141 | FollowMe.Index = 1; // 2st wp |
||
142 | //neu seit 0.28m |
||
143 | FollowMe.Type = POINT_TYPE_WP; // Typ des Wegpunktes |
||
144 | FollowMe.Name [0] = "F"; // Name des Wegpunktes (ASCII) |
||
145 | FollowMe.Name [1] = "o"; |
||
146 | FollowMe.Name [2] = "M"; |
||
147 | FollowMe.Name [3] = "e"; |
||
148 | FollowMe.WP_EventChannelValue = 0; // Will be transferred to the FC and can be used as Poti value there |
||
149 | FollowMe.AltitudeRate = 0; // rate to change the setpoint |
||
150 | FollowMe.Speed = FollowMeSpeed; // rate to change the Position |
||
151 | FollowMe.CamAngle = 255; // Camera servo angle in degree (255 -> POI-Automatic) |
||
152 | |||
153 | FollowMe.reserve[0] = 0; // reserve |
||
154 | FollowMe.reserve[1] = 0; // reserve |
||
155 | Request_SendFollowMe = 1; // triggers serial tranmission |
||
156 | } |
||
157 | else // no new position avalable (maybe bad gps signal condition) |
||
158 | { |
||
159 | FollowMe_Timer = SetDelay(100); // reset timer on higer frequency |
||
160 | } |
||
161 | LEDGRN_TOGGLE; // indication of active follow me |
||
162 | } |
||
163 | }break; |
||
164 | |||
165 | case(STATE_IDLE):LEDGRN_ON;break; |
||
166 | |||
167 | default:SysState = STATE_IDLE;break; |
||
168 | } |
||
169 | |||
170 | // restart ADConversion if ready |
||
171 | if(ADReady) |
||
172 | { |
||
173 | DebugOut.Analog[0] = Adc0; |
||
174 | DebugOut.Analog[1] = Adc1; |
||
175 | DebugOut.Analog[2] = Adc2; |
||
176 | DebugOut.Analog[3] = Adc3; |
||
177 | DebugOut.Analog[4] = Adc4; |
||
178 | DebugOut.Analog[5] = Adc5; |
||
179 | DebugOut.Analog[6] = Adc6; |
||
180 | DebugOut.Analog[7] = Adc7; |
||
181 | |||
182 | #ifdef USE_FOLLOWME |
||
183 | // AVcc = 5V --> 5V = 1024 counts |
||
184 | // the voltage at the voltage divider reference point is 0.8V less that the UBat |
||
185 | // because of the silicon diode inbetween. |
||
186 | // voltage divider R2=10K, R3=3K9 |
||
187 | // UAdc4 = R3/(R3+R2)*UBat= 3.9/(3.9+10)*UBat = UBat/3.564 |
||
188 | UBat = (3 * UBat + (64 * Adc4) / 368) / 4; |
||
189 | DebugOut.Analog[8] = UBat; |
||
190 | |||
191 | // check for zellenzahl |
||
192 | if(PowerOn < 100) |
||
193 | { |
||
194 | if(UBat<=84) Zellenzahl = 2; |
||
195 | else Zellenzahl = 3; |
||
196 | PowerOn++; |
||
197 | } |
||
198 | DebugOut.Analog[11] = Zellenzahl; |
||
199 | DebugOut.Analog[12] = PowerOn; |
||
200 | |||
201 | //show recognised Zellenzahl to user |
||
202 | if(i < Zellenzahl && PowerOn >= 100 && BeepTime == 0 && delay > 1000) |
||
203 | { |
||
204 | BeepTime = 100; |
||
205 | i++; |
||
206 | delay = 0; |
||
207 | } |
||
208 | if(delay < 1500) delay++; |
||
209 | |||
210 | |||
211 | // monitor battery undervoltage [...||(UBat<74) as temporary workaround to protect 2s lipo packs] |
||
212 | if(UBat < Zellenzahl * CELLUNDERVOLTAGE) |
||
213 | { // sound for low battery |
||
214 | BeepModulation = 0x0300; |
||
215 | if(!BeepTime) |
||
216 | { |
||
217 | BeepTime = 6000; // 0.6 seconds |
||
218 | } |
||
219 | Error |= ERROR_LOW_BAT; |
||
220 | } |
||
221 | else |
||
222 | { |
||
223 | Error &= ~ERROR_LOW_BAT; |
||
224 | } |
||
225 | #endif |
||
226 | ADReady = 0; |
||
227 | ADC_Enable(); // restart ad conversion sequence |
||
228 | } |
||
229 | |||
230 | // serial communication |
||
231 | USART0_ProcessRxData(); |
||
232 | USART0_TransmitTxData(); |
||
233 | |||
234 | // indicate error, blinking code tbd. |
||
235 | if(Error)LEDRED_ON; |
||
236 | else LEDRED_OFF; |
||
237 | DebugOut.Analog[13] = Error; |
||
238 | } |
||
239 | return (1); |
||
240 | } |
||
241 |