Subversion Repositories FlightCtrl

Rev

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

Rev Author Line No. Line
1538 killagreg 1
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2
// + Copyright (c) Holger Buss, Ingo Busker
3
// + Nur für den privaten Gebrauch
4
// + www.MikroKopter.com
5
// + porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
6
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
7
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
8
// + dass eine Nutzung (auch auszugsweise) nur für den privaten und nicht-kommerziellen Gebrauch zulässig ist.
9
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
10
// + bzgl. der Nutzungsbedingungen aufzunehmen.
11
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
12
// + Verkauf von Luftbildaufnahmen, usw.
13
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
14
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
15
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
16
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
17
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
18
// + auf anderen Webseiten oder Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
19
// + eindeutig als Ursprung verlinkt und genannt werden
20
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
21
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
22
// + Benutzung auf eigene Gefahr
23
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
24
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
25
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
26
// + mit unserer Zustimmung zulässig
27
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
28
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
29
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
30
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
31
// + this list of conditions and the following disclaimer.
32
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
33
// +     from this software without specific prior written permission.
34
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
35
// +     for non-commercial use (directly or indirectly)
36
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
37
// +     with our written permission
38
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
39
// +     clearly linked as origin
40
// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
41
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
42
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
43
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
44
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
45
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
46
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
47
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
48
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
49
// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
50
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
51
// +  POSSIBILITY OF SUCH DAMAGE.
52
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
53
#include <avr/boot.h>
54
 
55
#include <avr/io.h>
56
#include <avr/interrupt.h>
57
#include <util/delay.h>
58
 
59
#include "main.h"
60
#include "timer0.h"
61
#include "timer2.h"
62
#include "uart0.h"
63
#include "uart1.h"
64
#include "led.h"
65
#include "menu.h"
66
#include "fc.h"
67
#include "rc.h"
68
#include "analog.h"
69
#include "printf_P.h"
70
#ifdef USE_KILLAGREG
71
#include "mm3.h"
72
#endif
73
#ifdef USE_NAVICTRL
74
#include "spi.h"
75
#endif
76
#ifdef USE_MK3MAG
77
#include "mk3mag.h"
78
#endif
79
#include "twimaster.h"
80
#include "eeprom.h"
81
 
82
 
83
uint8_t BoardRelease = 10;
84
uint8_t CPUType = ATMEGA644;
85
uint8_t LowVoltageWarning = 94;
86
uint16_t FlightMinutes = 0, FlightMinutesTotal = 0;
87
 
88
uint8_t GetCPUType(void)
89
{   // works only after reset or power on when the registers have default values
90
        uint8_t CPUType = ATMEGA644;
91
        if( (UCSR1A == 0x20) && (UCSR1C == 0x06) ) CPUType = ATMEGA644P;  // initial Values for 644P after reset
92
        return CPUType;
93
}
94
 
95
 
96
uint8_t GetBoardRelease(void)
97
{
98
        uint8_t BoardRelease = 10;
99
        // the board release is coded via the pull up or down the 2 status LED
100
 
101
    PORTB &= ~((1 << PORTB1)|(1 << PORTB0)); // set tristate
102
    DDRB  &= ~((1 << DDB0)|(1 << DDB0)); // set port direction as input
103
 
104
        _delay_loop_2(1000); // make some delay
105
 
106
    switch( PINB & ((1<<PINB1)|(1<<PINB0)) )
107
    {
108
                case 0x00:
109
                        BoardRelease = 10; // 1.0
110
                        break;
111
                case 0x01:
112
                        BoardRelease = 11; // 1.1 or 1.2
113
                        break;
114
                case 0x02:
115
                        BoardRelease = 20; // 2.0
116
                        break;
117
                case 0x03:
118
                        BoardRelease = 13; // 1.3
119
                        break;
120
                default:
121
                        break;
122
        }
123
        // set LED ports as output
124
        DDRB |= (1<<DDB1)|(1<<DDB0);
125
        RED_ON;
126
        GRN_OFF;
127
        return BoardRelease;
128
}
129
 
130
void LipoDetection(uint8_t print)
131
{
132
        #define MIN_VOLTAGE_WARNING 93 // minimum low voltage warning to protect LiPO
133
        uint16_t timer;
134
        if(print) printf("\n\rBatt:");
135
        if(ParamSet.LowVoltageWarning < 50) // below 5.0 V the value is interpreted as single cell low volatage warning
136
        {
137
                timer = SetDelay(500); // wait at least 500 ms to get stable adc readings
138
                if(print) while(!CheckDelay(timer));
139
                if(UBat < 130) // less than 13.0 V must be a 3S Lipo
140
                {
141
                        LowVoltageWarning = 3 * ParamSet.LowVoltageWarning;
142
                        if(print)
143
                        {
144
                                Beep(3, 200);
145
                                printf(" 3 Cells  ");
146
                        }
147
                }
148
                else // <= 13.0V
149
                {
150
                        LowVoltageWarning = 4 * ParamSet.LowVoltageWarning;
151
                        if(print)
152
                        {
153
                                Beep(4, 200);
154
                                printf(" 4 Cells  ");
155
                        }
156
                }
157
        }
158
        else // classic settings
159
        {
160
                        LowVoltageWarning = ParamSet.LowVoltageWarning;
161
        }
162
        //if(LowVoltageWarning < MIN_VOLTAGE_WARNING) LowVoltageWarning = MIN_VOLTAGE_WARNING;
163
        if(print) printf("Low Warning level: %d.%dV", LowVoltageWarning/10, LowVoltageWarning%10);
164
}
165
 
166
int16_t main (void)
167
{
168
        uint16_t timer, flighttimer;
169
        uint8_t i;
170
 
171
        // disable interrupts global
172
        cli();
173
 
174
        // analyze hardware environment
175
        CPUType = GetCPUType();
176
        BoardRelease = GetBoardRelease();
177
 
178
        // disable watchdog
179
    MCUSR &=~(1<<WDRF);
180
    WDTCSR |= (1<<WDCE)|(1<<WDE);
181
    WDTCSR = 0;
182
 
183
    BeepTime = 2000;
184
 
185
        PPM_in[CH_GAS] = 0;
186
        StickYaw = 0;
187
        StickRoll = 0;
188
        StickNick = 0;
189
 
190
    RED_OFF;
191
 
192
        // initalize modules
193
        LED_Init();
194
    TIMER0_Init();
195
    TIMER2_Init();
196
        USART0_Init();
197
        if(CPUType == ATMEGA644P) USART1_Init();
198
    RC_Init();
199
        ADC_Init();
200
        I2C_Init();
201
        #ifdef USE_NAVICTRL
202
        SPI_MasterInit();
203
        #endif
204
        #ifdef USE_KILLAGREG
205
        MM3_Init();
206
        #endif
207
        #ifdef USE_MK3MAG
208
        MK3MAG_Init();
209
        #endif
210
 
211
        // enable interrupts global
212
        sei();
213
 
214
        printf("\n\r===================================");
215
        printf("\n\rFlightControl");
216
        printf("\n\rHardware: %d.%d", BoardRelease/10, BoardRelease%10);
217
        printf("\n\rthe use of this software is only permitted \n\ron original MikroKopter-Hardware");
218
        printf("\n\rwww.MikroKopter.de (c) HiSystems GmbH");
219
        printf("\n\r===================================");
220
 
221
        if(CPUType == ATMEGA644P)
222
        printf("\r\n     CPU: Atmega644p");
223
        else
224
        printf("\r\n     CPU: Atmega644");
225
        printf("\n\rSoftware: V%d.%d%c",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a');
226
        printf("\n\r===================================");
227
        GRN_ON;
228
 
229
        // Parameter Set handling
230
        ParamSet_Init();
231
 
232
        // Check connected BL-Ctrls
233
        printf("\n\rFound BL-Ctrl: ");
234
        motor_read = 0;
235
        UpdateMotor = 0;
236
        SendMotorData();
237
        while(!UpdateMotor);
238
        motor_read = 0;  // read the first I2C-Data
239
        timer = SetDelay(2000); // set timeout to 2 seconds
240
        for(i = 0; i < MAX_MOTORS; i++)
241
        {
242
                UpdateMotor = 0;
243
                SendMotorData();
244
                while(!UpdateMotor); // wait 2 ms to finish data transfer
245
                if(Mixer.Motor[i][MIX_GAS] > 0) // wait max 2 sec for the BL-Ctrls to wake up
246
                {
247
                        while(!CheckDelay(timer) && !(Motor[i].Present) ) // while not timeout and motor is not present
248
                        {
249
                                UpdateMotor = 0;
250
                                SendMotorData();
251
                                while(!UpdateMotor); // wait 2 ms
252
                        }
253
                }
254
                if(Motor[i].Present) printf("%d ",i+1);
255
        }
256
        for(i = 0; i < MAX_MOTORS; i++)
257
        {
258
                if(!Motor[i].Present && Mixer.Motor[i][MIX_GAS] > 0)
259
                {
260
                        printf("\n\r\n\r!! MISSING BL-CTRL: %d !!",i+1);
261
                        Servo_On(); // just in case the FC would be used as camera-stabilizer
262
                }
263
                Motor[i].Error = 0;
264
        }
265
        printf("\n\r===================================");
266
        SendMotorData();
267
 
268
        //wait for a short time (otherwise the RC channel check won't work below)
269
        timer = SetDelay(500);
270
        while(!CheckDelay(timer));
271
 
272
        if(ParamSet.Config0 & CFG0_AIRPRESS_SENSOR)
273
        {
274
                printf("\n\rCalibrating air pressure sensor..");
275
                timer = SetDelay(1000);
276
                SearchAirPressureOffset();
277
                while (!CheckDelay(timer));
278
                printf("OK\n\r");
279
        }
280
 
281
        #ifdef USE_NAVICTRL
282
        printf("\n\rSupport for NaviCtrl");
283
        #ifdef USE_RC_DSL
284
        printf("\r\nSupport for DSL RC at 2nd UART");
285
        #endif
286
        #ifdef USE_RC_SPECTRUM
287
        printf("\r\nSupport for SPEKTRUM RC at 2nd UART");
288
        #endif
289
        #endif
290
 
291
        #ifdef USE_KILLAGREG
292
        printf("\n\rSupport for MicroMag3 Compass");
293
        #endif
294
 
295
        #ifdef USE_MK3MAG
296
        printf("\n\rSupport for MK3MAG Compass");
297
        #endif
298
 
299
        #if (defined (USE_KILLAGREG) || defined (USE_MK3MAG))
300
        if(CPUType == ATMEGA644P) printf("\n\rSupport for GPS at 2nd UART");
301
        else                      printf("\n\rSupport for GPS at 1st UART");
302
        #endif
303
 
304
        // init variables
305
        SetNeutral(NO_ACC_CALIB);
306
 
307
        RED_OFF;
308
 
309
        BeepTime = 2000;
310
        ExternControl.Digital[0] = 0x55;
311
 
312
 
313
    FlightMinutes = GetParamWord(PID_FLIGHT_MINUTES);
314
    FlightMinutesTotal = GetParamWord(PID_FLIGHT_MINUTES_TOTAL);
315
        flighttimer = 0;
316
    if( (FlightMinutesTotal == 0xFFFF) || (FlightMinutes == 0xFFFF) )
317
        {
318
                FlightMinutes = 0;
319
                FlightMinutesTotal = 0;
320
        }
321
    printf("\n\rFlight-time %u min  Total:%u min", FlightMinutes, FlightMinutesTotal);
322
 
323
 
324
        printf("\n\rControl: ");
325
        if (ParamSet.Config0 & CFG0_HEADING_HOLD) printf("HeadingHold");
326
        else printf("Neutral (ACC-Mode)");
327
 
328
        LCD_Clear();
329
        I2CTimeout = 5000;
330
        LipoDetection(1);
331
        printf("\n\r===================================\n\r");
332
 
333
        timer = SetDelay(2000);
334
 
335
        // begin of main loop
336
        while (1)
337
        {
338
                if(UpdateMotor && ADReady)      // control interval
339
                {
340
                        UpdateMotor = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
341
 
342
                        //J4HIGH;
343
                        MotorControl();
344
                        //J4LOW;
345
 
346
                        SendMotorData(); // the flight control code
347
                        RED_OFF;
348
 
349
                        if(RC_Quality)  RC_Quality--;
350
                        else PPM_INPUT_ON; // if RC-Quality is lost, enable PPM input (could be disabled by a receiver on uart1)
351
 
352
                        #ifdef USE_NAVICTRL
353
                        if(NCDataOkay)
354
                        {
355
                                if(--NCDataOkay == 0) // no data from NC
356
                                {  // set gps control sticks neutral
357
                                        GPSStickNick = 0;
358
                                        GPSStickRoll = 0;
359
                                        NCSerialDataOkay = 0;
360
                                        NCGpsZ = 0;
361
                                }
362
                        }
363
                        #endif
364
 
365
                        if(!--I2CTimeout || MissingMotor) // try to reset the i2c if motor is missing ot timeout
366
                        {
367
                                RED_ON;
368
                                if(!I2CTimeout)
369
                                {
370
                                        I2C_Reset();
371
                                        I2CTimeout = 5;
372
                                        DebugOut.Analog[28]++; // I2C-Error
373
                                }
374
                                if((BeepModulation == 0xFFFF) && (MKFlags & MKFLAG_MOTOR_RUN) )
375
                                {
376
                                        BeepTime = 10000; // 1 second
377
                                        BeepModulation = 0x0080;
378
                                }
379
                        }
380
                        else
381
                        {
382
                                RED_OFF;
383
                        }
384
 
385
                        // allow Serial Data Transmit if motors must not updated or motors are not running
386
                        if( !UpdateMotor || !(MKFlags & MKFLAG_MOTOR_RUN) )
387
                        {
388
                                USART0_TransmitTxData();
389
                        }
390
                        USART0_ProcessRxData();
391
 
392
                        if(CheckDelay(timer))
393
                        {
394
                                timer += 20; // every 20 ms
395
                                if(PcAccess) PcAccess--;
396
                                else
397
                                {
398
                                        ExternControl.Config = 0;
399
                                        ExternStickNick= 0;
400
                                        ExternStickRoll = 0;
401
                                        ExternStickYaw = 0;
402
                                        if((BeepModulation == 0xFFFF) && (RC_Quality == 0))
403
                                        {
404
                                                BeepTime = 15000; // 1.5 seconds
405
                                                BeepModulation = 0x0C00;
406
                                        }
407
                                }
408
                                if(UBat < LowVoltageWarning)
409
                                {
410
                                        MKFlags |= MKFLAG_LOWBAT;
411
                                        BeepModulation = 0x0300;
412
                                        if(!BeepTime )
413
                                        {
414
                                                BeepTime = 6000; // 0.6 seconds
415
                                        }
416
                                }
417
                                else
418
                                {
419
                                        MKFlags &= ~MKFLAG_LOWBAT;
420
                                }
421
                                #ifdef USE_NAVICTRL
422
                                SPI_StartTransmitPacket();
423
                                SendSPI = 4;
424
                                #endif
425
 
426
                                if(!(MKFlags & MKFLAG_MOTOR_RUN)) flighttimer = 1450; // 0.5 minutes round up
427
                                if(++flighttimer == 2930)  // one minute
428
                                {
429
                                        flighttimer = 0;
430
                                        FlightMinutesTotal++;
431
                                        FlightMinutes++;
432
                                        SetParamWord(PID_FLIGHT_MINUTES_TOTAL, FlightMinutesTotal);
433
                                        SetParamWord(PID_FLIGHT_MINUTES, FlightMinutes);
434
                                        timer = SetDelay(20); // in case "timer += 20;" will not work
435
                            }
436
                        }// EOF CheckDelay(timer)
437
 
438
                        LED_Update();
439
                }
440
 
441
                #ifdef USE_NAVICTRL
442
                if(!SendSPI)
443
                {       // SendSPI is decremented in timer0.c with a rate of 9.765 kHz.
444
                        // within the SPI_TransmitByte() routine the value is set to 4.
445
                        // I.e. the SPI_TransmitByte() is called at a rate of 9.765 kHz/4= 2441.25 Hz,
446
                        // and therefore the time of transmission of a complete spi-packet (32 bytes) is 32*4/9.765 kHz = 13.1 ms.
447
                        SPI_TransmitByte();
448
                }
449
                #endif
450
        }
451
        return (1);
452
}
453