Subversion Repositories Projects

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1920 - 1
/*****************************************************************************
2
 *   Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de                  *
3
 *   Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net                  *
4
 *   Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net *
5
 *   Copyright (C) 2011 Harald Bongartz                                      *
6
 *   Copyright (C) 2012 Martin Runkel                                        *
7
 *                                                                           *
8
 *   This program is free software; you can redistribute it and/or modify    *
9
 *   it under the terms of the GNU General Public License as published by    *
10
 *   the Free Software Foundation; either version 2 of the License.          *
11
 *                                                                           *
12
 *   This program is distributed in the hope that it will be useful,         *
13
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of          *
14
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the           *
15
 *   GNU General Public License for more details.                            *
16
 *                                                                           *
17
 *   You should have received a copy of the GNU General Public License       *
18
 *   along with this program; if not, write to the                           *
19
 *   Free Software Foundation, Inc.,                                         *
20
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.               *
21
 *                                                                           *
22
 *                                                                           *
23
 *   Credits to:                                                             *
24
 *   Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN  *
25
 *                          http://www.mikrokopter.de                        *
26
 *   Gregor "killagreg" Stobrawa for his version of the MK code              *
27
 *   Thomas Kaiser "thkais" for the original project. See                    *
28
 *                          http://www.ft-fanpage.de/mikrokopter/            *
29
 *                          http://forum.mikrokopter.de/topic-4061-1.html    *
30
 *   Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code *
31
 *                          http://www.mylifesucks.de/oss/c-osd/             *
32
 *   Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility*
33
 *****************************************************************************/
34
 
35
 
36
#include "../cpu.h"
37
#include <avr/io.h>
38
#include <avr/interrupt.h>
39
#include <avr/pgmspace.h>
40
 
41
#include <string.h>
42
#include <stdlib.h>
43
 
44
#include "../lcd/lcd.h"
45
#include "../timer/timer.h"
46
#include "../tracking/servo.h"
47
#include "../messages.h"
48
#include "../lipo/lipo.h"
49
#include "stick.h"
50
#include "stick_setup.h"
51
#include "../eeprom/eeprom.h"
52
#include "../setup/setup.h"
53
 
54
#define SERVO_CORRECT 3.125
55
 
56
 
57
#include <util/delay.h>
58
#include <inttypes.h>
59
#include <stdlib.h>
60
 
61
#include "../main.h"
62
#include "../uart/uart1.h"
63
#include "../uart/usart.h"
64
 
65
 
66
#define LIMIT_MIN_MAX(value, min, max) {if(value <= min) value = min; else if(value >= max) value = max;}
67
 
68
        int16_t Pos_Stick[12];  // 1,5mS 
69
        int16_t Pos_alt[5];  //
70
 
71
        uint8_t BalkenPos = 0;
72
        uint8_t Stick_Display = 0;
73
        //uint8_t serialChannelRichtung = 0;
74
        //uint8_t serialChannelNeutral = 0;
75
        //uint8_t serialChannelConfig = 2;
76
 
77
 
78
//--------------------------------------------------------------
79
//
80
void joystick (void)
81
{
82
        // uint8_t chg = 0;
83
        // uint8_t Pos_Stick = 150;  // 1,5mS
84
        // uint8_t Pos_alt = 150;  // 
85
 
86
        //int16_t Pos_Stick[12];  // 1,5mS
87
 
88
        uint8_t chg = 0;
89
        //uint8_t BalkenPos = 0;
90
        uint8_t Stick_Nr = 0;
91
        //uint8_t Stick_Display = 0;
92
        uint8_t i = 0;
93
 
94
        memset (Pos_Stick, 150, 3); // füllt  3+1 Byte vom Pos_Stick[12] mit 150
95
 
96
        //int16_t Pos_alt[5];  // 
97
        int16_t Poti_Summe[5];  // 
98
        memset (Poti_Summe, 0, 5); // füllt  3+1 Byte mit 0
99
        int16_t Poti_Neutral[5];  // 
100
 
101
 
102
        // ADC- init
103
        //if (Config.Lipomessung == true)
104
        //{
105
 
106
        //}
107
        //for (uint8_t i = 0; i < 4; i++)
108
 
109
        if (Config.stick_typ[0] == 0) // Poti
110
                        {
111
                        PORTA &= ~(1<<PORTA0);
112
                        DDRA &= ~(1<<DDA0);
113
                        }
114
                else                    // Taster
115
                        {
116
                        PORTA |= (1<<PORTA0);
117
                        DDRA |= (1<<DDA0);
118
                        }
119
        if (Config.Lipomessung == false)
120
        {
121
        if (Config.stick_typ[1] == 0) // Poti
122
                                {
123
                                PORTA &= ~(1<<PORTA1);
124
                                DDRA &= ~(1<<DDA1);
125
                                }
126
                        else    // Taster
127
                                {
128
                                PORTA |= (1<<PORTA1);
129
                                DDRA |= (1<<DDA1);
130
                                }
131
        }
132
        if (Config.stick_typ[2] == 0)
133
                                {
134
                                PORTA &= ~(1<<PORTA2);
135
                                DDRA &= ~(1<<DDA2);
136
                                }
137
                        else
138
                                {
139
                                PORTA |= (1<<PORTA2);
140
                                DDRA |= (1<<DDA2);
141
                                }
142
        if (Config.stick_typ[3] == 0)
143
                                {
144
                                PORTA &= ~(1<<PORTA3);
145
                                DDRA &= ~(1<<DDA3);
146
                                }
147
                        else
148
                                {
149
                                PORTA |= (1<<PORTA3);
150
                                DDRA |= (1<<DDA3);
151
                                }
152
 
153
        //if (Config.stick_dir[i] == 0)  serialChannelRichtung &= ~(1<<i); else  serialChannelRichtung |= (1<<i);
154
        //if (Config.stick_neutral[i] == 0)  serialChannelNeutral &= ~(1<<i); else  serialChannelNeutral |= (1<<i);
155
 
156
        //if (Config.Lipomessung == true)
157
 
158
                //serialChannelConfig
159
 
160
 
161
// Ermittlung der Neutralposition
162
        for (uint8_t i = 0; i < 4; i++)
163
        {
164
                        ADMUX = (1<<REFS0)|(0<<MUX0);  // Multiplexer selection Register: AVCC with external capacitor at AREF pin , ADC1
165
                        ADMUX = (ADMUX & ~(0x1F)) | (i & 0x1F); // ADC[i] verwenden
166
 
167
                        timer = 50;
168
                        while (timer > 0);
169
 
170
                        ADCSRA = (1<<ADEN)|(1<<ADSC)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0); // ADC Enable, ADC Start, Prescaler 128
171
 
172
 
173
                        while (ADCSRA & (1<<ADSC));  // wenn ADC fertig
174
 
175
                                Poti_Neutral[i] = ((ADCW>>2)&0xff);
176
                                LIMIT_MIN_MAX (Poti_Neutral[i],108,148);
177
 
178
                                ADMUX = (ADMUX & ~(0x1F)) | (i & 0x1F); // ADC[i] verwenden
179
                                        // Stick_Nr 1,2,3 = Potis, Stick_Nr 1= Lipo
180
                                        ADCSRA |= (1<<ADSC); // ADC Start
181
 
182
                        while (ADCSRA & (1<<ADSC));  // wenn ADC fertig
183
 
184
                                        Poti_Neutral[i] = ((ADCW>>2)&0xff);
185
                                        LIMIT_MIN_MAX (Poti_Neutral[i],108,148);
186
        }
187
        Stick_Nr = 0;
188
        ADMUX = (ADMUX & ~(0x1F)) | (Stick_Nr & 0x1F); // ADC[i] verwenden
189
        ADCSRA |= (1<<ADSC); // ADC Start
190
 
191
        /*
192
        Stick_Nr = 0;
193
        ADMUX = (1<<REFS0)|(0<<MUX0);  // Multiplexer selection Register: AVCC with external capacitor at AREF pin , ADC1
194
        ADMUX = (ADMUX & ~(0x1F)) | (Stick_Nr & 0x1F); // ADC[Stick_Nr] verwenden
195
 
196
        timer = 50;
197
        while (timer > 0);
198
 
199
        ADCSRA = (1<<ADEN)|(1<<ADSC)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0); // ADC Enable, ADC Start, Prescaler 128
200
 
201
        // Stick-Neutralposition bestimmen
202
 
203
        while (ADCSRA & (1<<ADSC));  // wenn ADC fertig
204
 
205
                Poti_Neutral[Stick_Nr] = ((ADCW>>2)&0xff);
206
                LIMIT_MIN_MAX (Poti_Neutral[Stick_Nr],108,148);
207
                Stick_Nr = 2 ;
208
                ADMUX = (ADMUX & ~(0x1F)) | (Stick_Nr & 0x1F); // ADC[i] verwenden
209
                        // Stick_Nr 1,2,3 = Potis, Stick_Nr 1= Lipo
210
                        ADCSRA |= (1<<ADSC); // ADC Start
211
 
212
        while (ADCSRA & (1<<ADSC));  // wenn ADC fertig
213
 
214
                        Poti_Neutral[Stick_Nr] = ((ADCW>>2)&0xff);
215
                        LIMIT_MIN_MAX (Poti_Neutral[Stick_Nr],108,148);
216
                        Stick_Nr = 0;
217
                        ADMUX = (ADMUX & ~(0x1F)) | (Stick_Nr & 0x1F); // ADC[i] verwenden
218
                        // Stick_Nr 1,2,3 = Potis, Stick_Nr 1= Lipo
219
                        ADCSRA |= (1<<ADSC); // ADC Start
220
        */
221
 
222
 
223
 
224
        //OCR1A = 150 * SERVO_CORRECT;  // Servomitte
225
 
226
        lcd_cls ();
227
 
228
        // Kopfzeile und Rahmen zeichnen
229
 
230
        lcd_printp (PSTR(" serielle Potis 1-5 "), 2);
231
        //lcd_printp_at (7, 5, PSTR("%"), 0);
232
        //lcd_printp_at (16, 5, PSTR("mS"), 0);
233
 
234
    lcd_puts_at(0, 7, strGet(KEYLINE3), 0);
235
        lcd_printp_at (18, 7, PSTR("\x19O\x18"), 0);
236
 
237
 
238
        for (i=0 ; i< 5 ; i++)
239
        {
240
        BalkenPos = 12 + (i*8) ;
241
 
242
        lcd_rect(3,BalkenPos, 120, 6, 1);                       // +-150% Rahmen
243
        lcd_line(23,BalkenPos,23,(BalkenPos+6),1);      // -100%
244
        lcd_line(43,BalkenPos,43,(BalkenPos+6),1);      // -50%
245
        lcd_frect(62,BalkenPos, 2, 6, 1);                       // 0%
246
        lcd_line(83,BalkenPos,83,(BalkenPos+6),1);      // +50%
247
        lcd_line(103,BalkenPos,103,(BalkenPos+6),1);    // +100%
248
        }
249
 
250
        // Reset auf Mittelstellung
251
        Pos_Stick[0] = 150;
252
        Poti_Summe[0] = 0;
253
        Pos_Stick[2] = 150;
254
        Poti_Summe[2] = 0;
255
        Pos_Stick[4] = 150;
256
        Poti_Summe[4] = 0;
257
 
258
        chg = 255;
259
 
260
        do
261
        {
262
                if (!(ADCSRA & (1<<ADSC)))  // wenn ADC fertig
263
                {
264
 
265
                        //Pos_Stick[Stick_Nr] = 150 + 128 - ((ADCW>>2)&0xff);
266
                        //if (serialChannelRichtung & (1<<Stick_Nr))
267
                        if (Config.stick_dir[Stick_Nr] > 0)
268
                        Pos_Stick[Stick_Nr] = Poti_Neutral[Stick_Nr] - ((ADCW>>2)&0xff);
269
                        else
270
                        Pos_Stick[Stick_Nr] = ((ADCW>>2)&0xff) - Poti_Neutral[Stick_Nr];
271
 
272
 
273
                        LIMIT_MIN_MAX (Pos_Stick[Stick_Nr],-120,120);
274
                        //if ((Stick_Nr==0) || (Stick_Nr==2)) // nur die Potis 1,2 sind nicht neutralisierend
275
                        if (Config.stick_neutral[Stick_Nr]==0) //  nicht neutralisierend
276
                        {
277
                        Poti_Summe[Stick_Nr] += (Pos_Stick[Stick_Nr]/8) * abs(Pos_Stick[Stick_Nr]/8);
278
                        LIMIT_MIN_MAX (Poti_Summe[Stick_Nr],-(120*128),(120*128));
279
                        Pos_Stick[Stick_Nr]= Poti_Summe[Stick_Nr] / 128; // nicht neutralisierend
280
                        }
281
                        Pos_Stick[Stick_Nr] += 150;
282
                        //LIMIT_MIN_MAX (Pos_Stick[Stick_Nr],30,270); // war 75 , 225
283
                        LIMIT_MIN_MAX (Pos_Stick[Stick_Nr],Config.stick_min[Stick_Nr],Config.stick_max[Stick_Nr]); // war 30, 270
284
                        if (Pos_Stick[Stick_Nr] != Pos_alt[Stick_Nr]) // nur bei Änderung
285
                        {
286
                         chg |= (1<<Stick_Nr) ;
287
                        //Pos_alt=Pos_Stick ; // verschoben
288
                        }
289
                        Stick_Nr ++ ;
290
                        //if (Stick_Nr==1) Stick_Nr=2; // Lipo überspringen
291
 
292
                        if (Stick_Nr==3) // Taster
293
                        {
294
                                if (Config.stick_dir[Stick_Nr] > 0)
295
                                {
296
                                if (PINA & (1 << KEY_EXT))  Pos_Stick[Stick_Nr] = Config.stick_min[Stick_Nr];
297
                                else Pos_Stick[Stick_Nr] = Config.stick_max[Stick_Nr];
298
                                }
299
                                else
300
                                {
301
                                if (PINA & (1 << KEY_EXT))  Pos_Stick[Stick_Nr] = Config.stick_max[Stick_Nr];
302
                                else Pos_Stick[Stick_Nr] = Config.stick_min[Stick_Nr];
303
                                }
304
 
305
                                if (Pos_Stick[Stick_Nr] != Pos_alt[Stick_Nr])
306
                                {
307
                                chg |= (1<<Stick_Nr) ;
308
                                }
309
 
310
                         Stick_Nr=0;
311
                        }
312
 
313
                         /*
314
                         #ifndef ohne_Lipo // MartinR
315
                        Stick_Nr = 1; // MartinR AD-Kanal 1 überspringen wegen Lipo Überwachung
316
                        #endif
317
                        */
318
 
319
                        ADMUX = (ADMUX & ~(0x1F)) | (Stick_Nr & 0x1F); // ADC[i] verwenden
320
                        // Stick_Nr 1,2,3 = Potis, Stick_Nr 0= Lipo
321
                        ADCSRA |= (1<<ADSC); // ADC Start
322
 
323
                        //serialPotis ();
324
                }
325
 
326
 
327
 
328
                if ((get_key_press (1 << KEY_PLUS) | get_key_long_rpt_sp ((1 << KEY_PLUS), 3)))
329
                //if (get_key_press (1 << KEY_PLUS))
330
                //if (PINA & (1 << KEY_PLUS))
331
                {
332
                        if (Config.stick_neutral[4]==0) //  nicht neutralisierend
333
                        {
334
                        if (Config.stick_dir[4] == 0) Pos_Stick[4] ++ ; else Pos_Stick[4] -- ;
335
                        LIMIT_MIN_MAX (Pos_Stick[4],Config.stick_min[4],Config.stick_max[4]); // war 30, 270
336
                        }
337
                        else
338
                        {
339
                                if (Config.stick_dir[4] == 0) Pos_Stick[4] = Config.stick_max[4] ; else Pos_Stick[4] = Config.stick_min[4] ;
340
                        }
341
                        chg |= (1<<4) ;
342
                }      
343
 
344
 
345
                else if ((get_key_press (1 << KEY_MINUS) | get_key_long_rpt_sp ((1 << KEY_MINUS), 3)))
346
                {
347
                        if (Config.stick_neutral[4]==0) //  nicht neutralisierend
348
                        {
349
                        if (Config.stick_dir[4] == 0) Pos_Stick[4] -- ; else Pos_Stick[4] ++ ;
350
                        LIMIT_MIN_MAX (Pos_Stick[4],Config.stick_min[4],Config.stick_max[4]); // war 30, 270
351
                        //LIMIT_MIN_MAX (Pos_Stick[Stick_Nr],30,270); // war 75 , 225
352
                        }
353
                        else
354
                        {
355
                                if (Config.stick_dir[4] == 0) Pos_Stick[4] = Config.stick_min[4] ; else Pos_Stick[4] = Config.stick_max[4] ;
356
                        }
357
                        chg |= (1<<4) ;
358
                }      
359
 
360
                if (Config.stick_neutral[4]!= 0 && Pos_Stick[4]!= 150 && (PINA &(1 << KEY_PLUS)) && (PINA &(1 << KEY_MINUS)))
361
                {
362
                        Pos_Stick[4] = 150 ;
363
                        chg |= (1<<4) ;
364
                }
365
 
366
                if (get_key_press (1 << KEY_ENTER))
367
                {
368
                /*
369
                 for (i=0 ; i< 4 ; i++)
370
                        {
371
                        BalkenPos = 12 + (i*8) ;
372
 
373
                        lcd_frect (4, (BalkenPos+1), 118, 4, 0);  // Balken löschen
374
                        lcd_frect(62, BalkenPos, 2, 6, 1);  // 0%
375
                        }
376
                */     
377
                        Pos_Stick[0] = 150;
378
                        Poti_Summe[0] = 0;
379
                        Pos_Stick[2] = 150;
380
                        Poti_Summe[2] = 0;
381
                        Pos_Stick[4] = 150;
382
                        Poti_Summe[4] = 0;
383
                        BeepTime = 200;
384
                        BeepMuster = 0x0080;
385
                        chg = 255;
386
                }
387
 
388
                if (chg)
389
                {
390
                        if (chg & (1<<0)); // Stick 1
391
                                {
392
                                BalkenPos = 12 + (0*8) ;
393
                                Stick_Display = 0;
394
                                Balken_Zeichnen () ;
395
 
396
                                Pos_alt[Stick_Display]=Pos_Stick[Stick_Display];
397
                                }
398
                        // Stick 2 = Lipo
399
                        if (chg & (1<<1)); // Stick 2
400
                                {
401
                                BalkenPos = 12 + (1*8) ;
402
                                Stick_Display = 1;
403
                                //if (serialChannelConfig & (0<<1))  Balken_Zeichnen () ; // nur wenn keine Lipo-Spannung
404
                                if (Config.Lipomessung == false) Balken_Zeichnen () ; // nur wenn keine Lipo-Spannung
405
 
406
                                Pos_alt[Stick_Display]=Pos_Stick[Stick_Display];
407
                                }
408
 
409
 
410
                        if (chg & (1<<2)); // Stick 3
411
                                {
412
                                BalkenPos = 12 + (2*8) ;
413
                                Stick_Display = 2;
414
                                Balken_Zeichnen () ;
415
 
416
                                Pos_alt[Stick_Display]=Pos_Stick[Stick_Display];
417
                                }
418
 
419
                        if (chg & (1<<3)); // Stick 4 = Taster
420
                                {
421
                                BalkenPos = 12 + (3*8) ;
422
                                Stick_Display = 3;
423
                                Balken_Zeichnen () ;
424
 
425
                                Pos_alt[Stick_Display]=Pos_Stick[Stick_Display];
426
                                }
427
 
428
                        if (chg & (1<<4)); // Stick 5 = Taster vom PKT
429
                                {
430
                                BalkenPos = 12 + (4*8) ;
431
                                Stick_Display = 4;
432
                                Balken_Zeichnen () ;
433
                                //OCR1A = (((Pos_Stick[Stick_Display]-150)/1.6)+150) * SERVO_CORRECT;  // Servostellung , 1.6=0.8*0.5
434
 
435
                                Pos_alt[Stick_Display]=Pos_Stick[Stick_Display];
436
                                }
437
 
438
                        chg = 0;
439
                        serialPotis ();
440
                }
441
        }
442
        while (!get_key_press (1 << KEY_ESC));
443
        get_key_press(KEY_ALL);
444
 
445
        //#ifdef HWVERSION3_9
446
        //#ifndef ohne_Lipo // MartinR
447
        if (Config.Lipomessung == true) // MartinR: geändert
448
                        {
449
                        //DDRA &= ~(1<<DDA1);       // Eingang: PKT Lipo Messung
450
                        //PORTA &= ~(1<<PORTA1);
451
                        ADC_Init();             // ADC für Lipomessung wieder aktivieren
452
                        }
453
 
454
        //ADC_Init();             // ADC für Lipomessung wieder aktivieren
455
        //#endif
456
        //#endif
457
 
458
}
459
 
460
//--------------------------------------------------------------
461
//
462
void serialPotis (void)
463
{
464
        uint8_t i = 0;
465
 
466
        memset (buffer, 0, 12); // füllt die 12+1 Byte vom buffer mit 0
467
 
468
        for (i=0 ; i< 5 ; i++)
469
        {
470
        buffer[i] = Pos_Stick[i]-150 ;
471
        }
472
 
473
        SendOutData('y', ADDRESS_FC, 1, buffer, 12);
474
}
475
//--------------------------------------------------------------
476
//
477
void Balken_Zeichnen (void)
478
{
479
        // Balken löschen
480
 
481
                if ((Pos_Stick[Stick_Display] > Pos_alt[Stick_Display])&&(Pos_alt[Stick_Display] < 150)) // Balken links löschen
482
                lcd_frect ((63-((150 -Pos_alt[Stick_Display]) * 0.5)), (BalkenPos+1), (63-((150- Pos_Stick[Stick_Display]) * 0.5)), 4, 0);
483
                if ((Pos_Stick[Stick_Display] < Pos_alt[Stick_Display])&&(Pos_alt[Stick_Display] > 150)) // Balken rechts löschen
484
                lcd_frect ((63+((Pos_Stick[Stick_Display] - 150) * 0.5)), (BalkenPos+1), (63+((Pos_alt[Stick_Display] - 150) * 0.5)), 4, 0);
485
 
486
        // Balken zeichnen
487
                if (Pos_Stick[Stick_Display] >= 150)
488
                        {
489
                                lcd_frect (63, (BalkenPos+1), ((Pos_Stick[Stick_Display] - 150) * 0.5), 4, 1);
490
                                //write_ndigit_number_u (4, 5, ((Pos_Stick[Stick_Display] - 150) * 1.25), 3, 0, 0);  // Pulse width in %
491
                                lcd_frect(62, (BalkenPos), 2, 6, 1);  // 0%
492
                        }
493
                        else
494
                        {
495
                                lcd_frect (63 - ((150 - Pos_Stick[Stick_Display]) * 0.5), (BalkenPos+1), ((150 - Pos_Stick[Stick_Display]) * 0.5), 4, 1);
496
                                //write_ndigit_number_u (4, 5, ((150 - Pos_Stick[Stick_Display]) * 1.25), 3, 0, 0);  // Pulse width in %
497
                                lcd_frect(62, (BalkenPos), 2, 6, 1);  // 0%
498
                        }
499
        // Raster zeichnen              
500
                        lcd_line(3,  BalkenPos,3,  (BalkenPos+6),1);  // -150%
501
                        lcd_line(23, BalkenPos,23, (BalkenPos+6),1);  // -100%
502
                        lcd_line(43, BalkenPos,43, (BalkenPos+6),1);  // -50%
503
                        lcd_line(83, BalkenPos,83, (BalkenPos+6),1);  // +50%
504
                        lcd_line(103,BalkenPos,103,(BalkenPos+6),1);  // +100%
505
                        lcd_line(123,BalkenPos,123,(BalkenPos+6),1);  // +150%  
506
}
507