Subversion Repositories Projects

Rev

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