Subversion Repositories NaviCtrl

Rev

Rev 109 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
1 ingob 1
/*#######################################################################################*/
2
/* !!! THIS IS NOT FREE SOFTWARE !!!                                                     */
3
/*#######################################################################################*/
4
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5
// + Copyright (c) 2008 Ingo Busker, Holger Buss
171 ingob 6
// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY
1 ingob 7
// + FOR NON COMMERCIAL USE ONLY
8
// + www.MikroKopter.com
9
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
41 ingob 10
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
11
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
12
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
13
// + bzgl. der Nutzungsbedingungen aufzunehmen.
1 ingob 14
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
15
// + Verkauf von Luftbildaufnahmen, usw.
16
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
41 ingob 17
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
1 ingob 18
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
19
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
20
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
21
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
22
// + eindeutig als Ursprung verlinkt werden
23
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
24
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
25
// + Benutzung auf eigene Gefahr
26
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
27
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
171 ingob 28
// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
1 ingob 29
// + mit unserer Zustimmung zulässig
30
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
31
// + Teile, die nicht eindeutig mit unserer Lizenzvereinbarung gekennzeichnet sind, unterliegen u.U. eigenen Vereinbarungen:
41 ingob 32
// + z.B. Die Funktion printf_P() unterliegt ihrer eigenen Lizenz
1 ingob 33
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
41 ingob 34
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
1 ingob 35
// + this list of conditions and the following disclaimer.
36
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
37
// +     from this software without specific prior written permission.
41 ingob 38
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permitted
1 ingob 39
// +     for non-commercial use (directly or indirectly)
41 ingob 40
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
1 ingob 41
// +     with our written permission
41 ingob 42
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
43
// +     clearly linked as origin
171 ingob 44
// +   * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
1 ingob 45
//
46
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
47
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
48
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
49
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
50
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
51
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
52
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
53
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
54
// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
55
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
41 ingob 56
// +  POSSIBILITY OF SUCH DAMAGE.
1 ingob 57
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
41 ingob 58
//#define MCLK96MHZ
1 ingob 59
const unsigned long _Main_Crystal = 25000;
41 ingob 60
#include <stdio.h>
61
#include "91x_lib.h"
62
#include "led.h"
63
#include "uart0.h"
64
#include "uart1.h"
65
#include "uart2.h"
66
#include "GPS.h"
67
#include "i2c.h"
68
#include "timer.h"
69
#include "spi_slave.h"
70
#include "fat16.h"
71
#include "usb.h"
72
#include "sdc.h"
82 killagreg 73
#include "logging.h"
88 killagreg 74
#include "settings.h"
1 ingob 75
#include "main.h"
76
 
77
u32 TimerCompassUpdate;
41 ingob 78
u32 TimerCheckError;
79
u32 ErrorCode = 0;
80
u16 BeepTime;
92 killagreg 81
u8  NCFlags = 0;
1 ingob 82
 
41 ingob 83
u8 ClearMKFlags = 0;
84
u8 StopNavigation = 0;
85
Param_t Parameter;
78 holgerb 86
volatile FC_t FC;
1 ingob 87
 
41 ingob 88
s8 ErrorMSG[25];
89
 
1 ingob 90
//----------------------------------------------------------------------------------------------------
41 ingob 91
void SCU_Config(void)
1 ingob 92
{
41 ingob 93
        /* configure PLL and set it as master clock source */
94
        SCU_MCLKSourceConfig(SCU_MCLK_OSC);             // set master clock source to external oscillator clock (25MHz) before diabling the PLL
95
        SCU_PLLCmd(DISABLE);                                    // now disable the PLL
96
        #ifdef MCLK96MHZ
97
        SCU_BRCLKDivisorConfig(SCU_BRCLK_Div2); // set BRCLK to MCLK/2 = 48MHz
98
        SCU_PCLKDivisorConfig(SCU_PCLK_Div4);   // set PCLK     (APB bus clock) divisor to 4 (half Reference Clock)
99
        SCU_RCLKDivisorConfig(SCU_RCLK_Div2);   // set RCLK     (Reference Clock) divisor to 1 (full PPL clock)
100
        SCU_HCLKDivisorConfig(SCU_HCLK_Div2);   // set HCLK     (AHB bus clock) divisor to 1 (full Reference Clock)
101
        SCU_PLLFactorsConfig(192,25,2);                 // PLL = 96 MHz, Feedback Divider N=192, Pre-Divider M=25, Post-Divider P=2
102
        #else
103
        SCU_BRCLKDivisorConfig(SCU_BRCLK_Div1); // set BRCLK to MCLK = 48MHz
104
        SCU_PCLKDivisorConfig(SCU_PCLK_Div2);   // set PCLK     (APB bus clock) divisor to 2 (half Reference Clock)
105
        SCU_RCLKDivisorConfig(SCU_RCLK_Div1);   // set RCLK     (Reference Clock) divisor to 1 (full PPL clock)
106
        SCU_HCLKDivisorConfig(SCU_HCLK_Div1);   // set HCLK     (AHB bus clock) divisor to 1 (full Reference Clock)
107
        SCU_PLLFactorsConfig(192,25,3);                 // PLL = 48 MHz, Feedback Divider N=192, Pre-Divider M=25, Post-Divider P=3
108
        #endif
109
        SCU_PLLCmd(ENABLE);                                     // Enable PLL (is disabled by SCU_PLLFactorsConfig)
110
        SCU_MCLKSourceConfig(SCU_MCLK_PLL);     // set master clock source to PLL
111
}
1 ingob 112
 
41 ingob 113
//----------------------------------------------------------------------------------------------------
114
void GetNaviCtrlVersion(void)
1 ingob 115
{
41 ingob 116
        u8 msg[25];
1 ingob 117
 
41 ingob 118
        sprintf(msg,"\n\r NaviCtrl V%d.%d%c",  VERSION_MAJOR,  VERSION_MINOR, 'a'+ VERSION_PATCH);
119
        SerialPutString(msg);
1 ingob 120
}
121
 
41 ingob 122
 
1 ingob 123
//----------------------------------------------------------------------------------------------------
41 ingob 124
s8* GenerateGPSLogFileName(void)
1 ingob 125
{
41 ingob 126
        static u16 filenum = 0; // file name counter
127
        static s8 filename[30];
128
        static DateTime_t LastTime = {0,0,0,0,0,0,0,0};
1 ingob 129
 
41 ingob 130
        if(SystemTime.Valid)
131
        {
132
                // if the day has been changed
133
                if((LastTime.Year != SystemTime.Year) || (LastTime.Month != SystemTime.Month) || (LastTime.Day != SystemTime.Day))
134
                {
135
                        LastTime.Year = SystemTime.Year;
136
                        LastTime.Month = SystemTime.Month;
137
                        LastTime.Day = SystemTime.Day;
138
                        LastTime.Valid = 1;
139
                        filenum = 0; // reset file counter
140
                }
141
                sprintf(filename, "LOG/%04i%02i%02i/GPS%05i.KML", SystemTime.Year, SystemTime.Month, SystemTime.Day, filenum);
142
                filenum++;
143
                return filename;
144
        }
145
        else return NULL;
146
}
24 StephanB 147
 
148
 
41 ingob 149
//----------------------------------------------------------------------------------------------------
150
void CheckErrors(void)
151
{
152
        if(!CheckSPIOkay)
153
        {
154
                LED_RED_ON;
155
                sprintf(ErrorMSG,"no FC communication ");
156
                ErrorCode = 3;
157
                StopNavigation = 1;
158
        }
159
        else if(!CheckI2COkay)
160
        {
161
                LED_RED_ON;
162
                sprintf(ErrorMSG,"no MK3Mag communication ");
163
                ErrorCode = 4;
164
                StopNavigation = 1;
165
        }
166
        else if(FC_Version.Compatible != FC_SPI_COMPATIBLE)
167
        {
168
                LED_RED_ON;
169
                sprintf(ErrorMSG,"FC not compatible ");
170
                ErrorCode = 1;
171
                StopNavigation = 1;
172
        }
173
        else if(MK3MAG_Version.Compatible != MK3MAG_I2C_COMPATIBLE)
174
        {
175
                sprintf(ErrorMSG,"MK3Mag not compatible ");
176
                LED_RED_ON;
177
                ErrorCode = 2;
178
                StopNavigation = 1;
179
        }
180
        else if(!CheckGPSOkay)
181
        {
182
                LED_RED_ON;
183
                sprintf(ErrorMSG,"no GPS communication ");
184
                ErrorCode = 5;
185
                StopNavigation = 1;
186
        }
187
        else if(I2C_Heading.Heading < 0)
188
        {
189
                LED_RED_ON;
190
                sprintf(ErrorMSG,"bad compass value ");
191
                ErrorCode = 6;
192
                StopNavigation = 1;
193
        }
194
        else if(FC.RC_Quality < 100)
195
        {
196
                LED_RED_ON;
197
                sprintf(ErrorMSG,"RC Signal lost ");
198
                ErrorCode = 7;
199
        }
200
        else // no error occured
201
        {
202
                sprintf(ErrorMSG,"No Error               ");
203
                ErrorCode = 0;
204
                StopNavigation = 0;
205
                LED_RED_OFF;
206
        }
207
        CheckSPIOkay = 0;
208
        CheckGPSOkay = 0;
209
        CheckI2COkay = 0;
210
}
24 StephanB 211
 
212
 
1 ingob 213
 
41 ingob 214
//----------------------------------------------------------------------------------------------------
215
int main(void)
216
{
217
        /* Configure the system clocks */
218
        SCU_Config();
219
        /* init VIC (Vectored Interrupt Controller)     */
220
        SCU_AHBPeriphClockConfig(__VIC,ENABLE); // enable AHB bus clock for VIC
221
        SCU_AHBPeriphReset(__VIC, DISABLE);             // disable reset state for VIC
222
        VIC_DeInit();                                                   // deinitializes the VIC module registers to their default reset values.
223
        // initialize the interrupt handler
224
        Interrupt_Init();
225
        // initialize timer 1
226
        TIMER1_Init();
227
        // initialize the LEDs (needs Timer 1)
228
        Led_Init();
229
        // initialize the debug UART1
230
        UART1_Init();
231
        SerialPutString("\r\n---------------------------------------------");
232
        // initialize UART2 to FLIGHTCTRL
233
        UART2_Init();
234
        // initialize UART0 (to MKGPS or MK3MAG)
235
        UART0_Init();
236
        // initialize usb
237
        USB_ConfigInit();
238
        // initialize SPI0 to FC
239
        SPI0_Init();
240
        // initialize i2c bus to MK3MAG (needs Timer 1)
241
        I2C1_Init();
242
        // initialize the gps position controller (needs Timer 1)
243
        GPS_Init();
244
        // initialize fat16 partition on sd card (needs Timer 1)
245
        Fat16_Init();
88 killagreg 246
        // initialize the settings
247
        Settings_Init();
92 killagreg 248
        // initialize logging (needs settings)
82 killagreg 249
        Logging_Init();
1 ingob 250
 
41 ingob 251
        TimerCompassUpdate = SetDelay(5);
252
        TimerCheckError = SetDelay(3000);
253
        SerialPutString("\r\n---------------------------------------------");
254
        SerialPutString("\n\r Version information:");
255
        GetNaviCtrlVersion();
256
        SPI0_GetFlightCtrlVersion();
257
        if(FC_Version.Compatible != FC_SPI_COMPATIBLE)
258
        {
259
                //sprintf(ErrorMSG,"\r\n %d <-> %d", FlightCtrlVersion.NaviCompatible, FC_SPI_COMPATIBLE);
260
                //SerialPutString(ErrorMSG);
261
                SerialPutString("\n\r Flight-Ctrl not compatible");
262
                LED_RED_ON;
263
        }
78 holgerb 264
 
41 ingob 265
        I2C1_GetMK3MagVersion();
266
        if(MK3MAG_Version.Compatible != MK3MAG_I2C_COMPATIBLE)
267
        {
268
                SerialPutString("\n\r MK3Mag not compatible");
269
                LED_RED_ON;
270
        }
1 ingob 271
 
79 killagreg 272
        SerialPutString("\r\n---------------------------------------------\r\n\r\n");
24 StephanB 273
 
41 ingob 274
        for (;;)
275
        {
92 killagreg 276
                UART1_ProcessRxData();  // process request
41 ingob 277
                UART1_TransmitTxData();         // send answer
24 StephanB 278
 
41 ingob 279
                SPI0_UpdateBuffer();
49 ingob 280
                UART0_Transmit(); // empty txd buffer
41 ingob 281
                UART1_Transmit(); // empty txd buffer
24 StephanB 282
 
41 ingob 283
                // ------------------------- I2C Timing --------------------------------
284
                if(I2C_State != I2C_OFF)
285
                {
286
                        if (CheckDelay(TimerCompassUpdate))
287
                        {
288
                                // check for hanging I2C bus
289
                                if(CheckDelay(I2C1_Timeout))
290
                                {       // reset I2C
291
                                        SerialPutString("\n\rCommunication-Error: MK3Mag\n\r");
292
                                        I2C1_Deinit();
293
                                        I2C1_Init();
294
                                }
295
                                else
296
                                {       // check for incomming compass calibration request
297
                                        // update CalByte from spi input queue
298
                                        fifo_get(&CompassCalcStateFiFo, (u8 *)&(I2C_WriteCal.CalByte));
299
                                        // send new calstate
300
                                        if(I2C_ReadCal.CalByte != I2C_WriteCal.CalByte)
301
                                        {
302
                                                I2C1_SendCommand(I2C_CMD_WRITE_CAL);
303
                                        }
304
                                        else // request current heading
305
                                        {
306
                                                I2C1_SendCommand(I2C_CMD_READ_HEADING);
307
                                        }
308
                                        TimerCompassUpdate = SetDelay(25);    // every 25 ms
309
                                }
24 StephanB 310
 
41 ingob 311
                        }
312
                }
1 ingob 313
 
41 ingob 314
                // ---------------- Error Check Timing ----------------------------
315
                if(CheckDelay(TimerCheckError))
1 ingob 316
                {
41 ingob 317
                        if(!CheckSPIOkay) GPS_Navigation(); // process the GPS data even if the FC is not connected
318
                        CheckErrors();
77 holgerb 319
                        if(FC.MKFlags & MKFLAG_FLY) NaviData.FlyingTime++; // we want to count the battery-time
320
//                      else NaviData.FlyingTime = 0;
41 ingob 321
                        TimerCheckError = SetDelay(1000);
322
                        Request_SendFollowMe = TRUE;
92 killagreg 323
                        if(SerialLinkOkay) SerialLinkOkay--;
324
                        if(SerialLinkOkay < 250 - 5) NCFlags |= NC_FLAG_NOSERIALLINK; // 5 seconds timeout for serial communication
325
                        else NCFlags &= ~NC_FLAG_NOSERIALLINK;
1 ingob 326
                }
82 killagreg 327
                // ---------------- Logging  ---------------------------------------
328
                Logging_Update();
24 StephanB 329
        }
1 ingob 330
}
331
 
24 StephanB 332