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
 *                                                                           *
7
 *   This program is free software; you can redistribute it and/or modify    *
8
 *   it under the terms of the GNU General Public License as published by    *
9
 *   the Free Software Foundation; either version 2 of the License.          *
10
 *                                                                           *
11
 *   This program is distributed in the hope that it will be useful,         *
12
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of          *
13
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the           *
14
 *   GNU General Public License for more details.                            *
15
 *                                                                           *
16
 *   You should have received a copy of the GNU General Public License       *
17
 *   along with this program; if not, write to the                           *
18
 *   Free Software Foundation, Inc.,                                         *
19
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.               *
20
 *                                                                           *
21
 *                                                                           *
22
 *   Credits to:                                                             *
23
 *   Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN  *
24
 *                          http://www.mikrokopter.de                        *
25
 *   Gregor "killagreg" Stobrawa for his version of the MK code              *
26
 *   Thomas Kaiser "thkais" for the original project. See                    *
27
 *                          http://www.ft-fanpage.de/mikrokopter/            *
28
 *                          http://forum.mikrokopter.de/topic-4061-1.html    *
29
 *   Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code *
30
 *                          http://www.mylifesucks.de/oss/c-osd/             *
31
 *   Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility*
32
 *****************************************************************************/
33
 
34
 
35
#include "../cpu.h"
36
#include <avr/io.h>
37
#include <avr/interrupt.h>
38
#include <avr/pgmspace.h>
39
#include <string.h>
40
#include <stdlib.h>
41
#include "../main.h"
42
#include "motortest.h"
43
#include "../lcd/lcd.h"
44
#include "../timer/timer.h"
45
#include "../motortest/twimaster.h"
46
//#include "menu.h"
47
#include "../uart/uart1.h"
48
#include "../uart/usart.h"
49
#include "../messages.h"
50
 
51
 
52
 
53
 
54
uint8_t m;
55
uint8_t mmode; // 0=Value 1=Motor
56
uint8_t v;
57
 
58
volatile uint8_t i2c_state;
59
volatile uint8_t motor_addr = 0;
60
 
61
 
62
//--------------------------------------------------------------
63
// Senden der Motorwerte per I2C-Bus
64
//
65
void SendMotorData(uint8_t m,uint8_t v)
66
{
67
        if (m==0)
68
 
69
                for(m=0;m<MAX_MOTORS;m++)   // alle Motoren
70
                {
71
//                      Motor[m].SetPoint = MotorTest[m];
72
                        Motor[m].SetPoint = v;
73
                        Motor[m].SetPointLowerBits = 0;
74
 
75
//                      Motor[i].SetPoint = MotorTest[i] / 4;                   // testing the high resolution
76
//                      Motor[i].SetPointLowerBits = MotorTest[i] % 4;
77
 
78
                }
79
        else
80
        {
81
                Motor[m-1].SetPoint = v;
82
                Motor[m-1].SetPointLowerBits = 0;
83
        }
84
 
85
 
86
 
87
        if(I2C_TransferActive)
88
                I2C_TransferActive = 0; // enable for the next time
89
        else
90
        {
91
                motor_write = 0;
92
                I2C_Start(TWI_STATE_MOTOR_TX); //Start I2C Interrupt Mode
93
        }
94
}
95
 
96
 
97
//--------------------------------------------------------------
98
//
99
void Search_BL (void)
100
{
101
        uint8_t i = 0;
102
        unsigned int timer;
103
        lcd_cls ();
104
        MotorenEin =0;
105
        MotorTest[i] = 0;
106
 
107
        lcd_printp (PSTR("Suche BL-Ctrl"), 0);
108
 
109
        // Check connected BL-Ctrls
110
        BLFlags |= BLFLAG_READ_VERSION;
111
        motor_read = 0;  // read the first I2C-Data
112
 
113
        SendMotorData(0,0);
114
        timer = SetDelay(1);
115
        while(!(BLFlags & BLFLAG_TX_COMPLETE) && !CheckDelay(timer)); //wait for complete transfer
116
 
117
        timer = SetDelay(1);
118
        for(i=0; i < MAX_MOTORS; i++)
119
        {
120
 
121
                SendMotorData(i,0);
122
 
123
 
124
                while(!(BLFlags & BLFLAG_TX_COMPLETE)  && !CheckDelay(timer)); //wait for complete transfer
125
 
126
 
127
                if(Motor[i].State & MOTOR_STATE_PRESENT_MASK)
128
                {
129
 
130
                        lcd_printp_at (0, 1, PSTR("Found BL-Ctrl:"), 0);
131
                        lcd_print_hex_at (18,1,i,0);
132
 
133
                        lcd_printp_at (0, 2, PSTR("Version:"), 0);
134
                        lcd_print_hex_at (8,2,Motor[i].Version,0);
135
                        lcd_printp_at (11, 2, PSTR("SetPoi:"), 0);
136
                        lcd_print_hex_at (18,2,Motor[i].SetPoint,0);
137
 
138
                        lcd_printp_at (0, 3, PSTR("SetPoiL:"), 0);
139
                        lcd_print_hex_at (8,3,Motor[i].SetPointLowerBits,0);
140
                        lcd_printp_at (11, 3, PSTR("State :"), 0);
141
                        lcd_print_hex_at (18,3,Motor[i].State,0);
142
 
143
                        lcd_printp_at (0, 4, PSTR("ReadMod:"), 0);
144
                        lcd_print_hex_at (8,4,Motor[i].ReadMode,0);
145
                        lcd_printp_at (11, 4, PSTR("Currnt:"), 0);
146
                        lcd_print_hex_at (18,4,Motor[i].Current,0);
147
 
148
                        lcd_printp_at (0, 5, PSTR("MaxPWM :"), 0);
149
                        lcd_print_hex_at (8,5,Motor[i].MaxPWM,0);
150
                        lcd_printp_at (11, 5, PSTR("Temp  :"), 0);
151
                        write_ndigit_number_u (18,5,Motor[i].Temperature,3,1,0);
152
                }
153
 
154
        } //End For I
155
}
156
 
157
 
158
//--------------------------------------------------------------
159
//
160
void motor (uint8_t m,uint8_t v)
161
{
162
        memset (buffer, 0, 16);
163
 
164
        if(m == 0)
165
        {
166
                memset (buffer, v, 16);
167
        }
168
        else
169
        {
170
                buffer[m-1] = v;
171
        }
172
 
173
        SendOutData('t', ADDRESS_FC, 1, buffer, 16);
174
}
175
 
176
//--------------------------------------------------------------
177
//
178
void motor_test (uint8_t MotorMode)
179
{
180
 
181
        lcd_cls ();
182
        mmode = 1; // 1=Motor
183
        m = 1;
184
        v = 0;
185
        char buffer[7];
186
        unsigned int SerLoop;
187
        SerLoop = 10;
188
 
189
        if (MotorMode == I2C_Mode)
190
        {
191
                Search_BL();
192
                do
193
                {
194
                        lcd_printp_at (11, 7, PSTR("Ende Check"), 0);
195
                        if (get_key_press (1 << KEY_ESC))
196
                        {
197
                                get_key_press(KEY_ALL);
198
                                return;
199
                        }
200
                }
201
                while (!get_key_press (1 << KEY_ENTER));
202
        }
203
 
204
        lcd_cls();
205
        lcd_printp (PSTR(" BL-Ctrl Test        "), 2);
206
        lcd_printp_at (2, 2, PSTR("Motor:   1"), 0);
207
        lcd_printp_at (2, 3, PSTR("Value:   0"), 0);
208
        lcd_frect ((8*1), (8*5), (0 * (14*8)) / 255, 6, 1);
209
//      lcd_printp_at (0, 7, PSTR(KEY_LINE_3), 0);
210
        lcd_puts_at(0, 7, strGet(KEYLINE3), 0);
211
        lcd_printp_at (18, 7, PSTR("\x1a \x1b"), 0);
212
        lcd_printp_at (0, 2, PSTR("\x1d"), 0);
213
 
214
#if defined HWVERSION1_3W || defined HWVERSION1_3 || defined HWVERSION3_9
215
        if (MotorMode == I2C_Mode)
216
                uart1_puts("Motor;Version;Setpoint high;Setpoint low;State;ReadMode;Current;MaxPWM;Temperature\r");
217
#endif
218
#if defined HWVERSION1_2W || defined HWVERSION1_2
219
        if (MotorMode == I2C_Mode)
220
                USART_puts("Motor;Version;Setpoint high;Setpoint low;State;ReadMode;Current;MaxPWM;Temperature\r");
221
#endif
222
 
223
        if (MotorMode == FC_Mode)
224
        {
225
                if (hardware == NC && current_hardware == NC)
226
                {
227
                        SwitchToFC();
228
                }
229
        }
230
        do
231
        {
232
                // mmode 0=Value 1=Motor
233
 
234
                if ((mmode == 0) && (get_key_press (1 << KEY_PLUS) || get_key_long_rpt_sp ((1 << KEY_PLUS), 3)) && (v < 254))
235
                {
236
                        v++;
237
                        write_ndigit_number_u (9, 3, v, 3, 0,0);
238
                        if (MotorMode == FC_Mode)
239
                                lcd_frect ((8*1), (8*5), (v * (14*8)) / 255, 6, 1);
240
                }
241
 
242
                if ((mmode == 0) && (get_key_press (1 << KEY_MINUS) || get_key_long_rpt_sp ((1 << KEY_MINUS), 3)) && (v > 0))
243
                {
244
                        if (MotorMode == FC_Mode)
245
                                lcd_frect (((v * (14*8) / 255) + 8), (8*5), ((14*8) / 255), 6, 0);
246
 
247
                        v--;
248
                        write_ndigit_number_u (9, 3, v, 3, 0,0);
249
                        if (MotorMode == FC_Mode)
250
                                lcd_frect ((8*1), (8*5), (v * (14*8)) / 255, 6, 1);
251
                }
252
 
253
                if ((mmode == 1) && (get_key_press (1 << KEY_PLUS) || get_key_long_rpt_sp ((1 << KEY_PLUS), 1)) && (m < 16))
254
                {
255
                        m++;
256
                        write_ndigit_number_u (9, 2, m, 3, 0,0);
257
                }
258
 
259
                if ((mmode == 1) && (get_key_press (1 << KEY_MINUS) || get_key_long_rpt_sp ((1 << KEY_MINUS), 1)) && (m > 0))
260
                {
261
                        m--;
262
                        if(m > 0)
263
                                write_ndigit_number_u (9, 2, m, 3, 0,0);
264
                        if(m == 0)
265
                                lcd_printp_at (9, 2, PSTR("All"), 0);
266
                }
267
 
268
                if (get_key_press (1 << KEY_ENTER))
269
                {
270
                        if (MotorMode == I2C_Mode)
271
                        {
272
                                if (v > 0)
273
                                {
274
                                        m = 0;
275
                                        v=0;
276
                                        lcd_frect ((8*1), (8*5), (0 * (14*8)) / 255, 6, 1);
277
                                        lcd_cls_line (0, 5, 21);
278
                                        if(m > 0) write_ndigit_number_u (9, 2, m, 3, 0,0);
279
                                        if(m == 0) lcd_printp_at (9, 2, PSTR("All"), 0);
280
                                        write_ndigit_number_u (9, 3, v, 3, 0,0);
281
                                        SendMotorData(m,v);
282
                                        timer = SetDelay(1);
283
                                        while(!(BLFlags & BLFLAG_TX_COMPLETE)  && !CheckDelay(timer)); //wait for complete transfer
284
                                }
285
                        }
286
 
287
                        if(mmode == 0) // 0=Value
288
                        {
289
                                lcd_printp_at (0, 2, PSTR("\x1d"), 0);
290
                                lcd_printp_at (0, 3, PSTR(" "), 0);
291
                                mmode = 1; // 1=Motor
292
                        }
293
                        else
294
                        {
295
                                lcd_printp_at (0, 2, PSTR(" "), 0);
296
                                lcd_printp_at (0, 3, PSTR("\x1d"), 0);
297
                                mmode = 0; // 0=Value
298
                        }
299
                }
300
                //if (get_key_press (1 << KEY_ENTER))//
301
 
302
                if (MotorMode == I2C_Mode)
303
                {
304
                        SendMotorData(m,v);
305
                        timer = SetDelay(1);
306
                        lcd_printp_at (0, 3, PSTR("SetPoint   :"), 0);
307
 
308
                        write_ndigit_number_u (13,3,Motor[m-1].SetPoint,3,0,0);
309
                        lcd_printp_at (0, 4, PSTR("Current    :"), 0);
310
                        lcd_print_hex_at (13,4,Motor[m-1].Current,0);
311
                        write_ndigit_number_u (13,4,Motor[m-1].Current,3,0,0);
312
                        lcd_printp_at (0, 5, PSTR("Temperature:"), 0);
313
                        write_ndigit_number_u (13,5,Motor[m-1].Temperature,3,0,0);
314
                        lcd_printp_at (0, 6, PSTR("Version:"), 0);
315
                        lcd_print_hex_at (8,6,Motor[m-1].Version,0);
316
                        lcd_printp_at (11, 6, PSTR("State :"), 0);
317
                        lcd_print_hex_at (18,6,Motor[m-1].State,0);
318
 
319
#if defined HWVERSION1_3W || defined HWVERSION1_3 || defined HWVERSION3_9
320
 
321
                        if (Motor[m-1].SetPoint > 0)
322
                        {
323
                                if (SerLoop == 0)
324
                                {
325
                                        itoa( m-1, buffer, 10);         // convert interger into string (decimal format)
326
                                        uart1_puts(buffer);                     // and transmit string to UART
327
                                        uart1_puts(";");
328
                                        itoa( Motor[m-1].Version, buffer, 10);   //
329
                                        uart1_puts(buffer);
330
                                        uart1_puts(";");
331
                                        itoa( Motor[m-1].SetPoint, buffer, 10);   //
332
                                        uart1_puts(buffer);
333
                                        uart1_puts(";");
334
                                        itoa( Motor[m-1].SetPointLowerBits, buffer, 10);   //
335
                                        uart1_puts(buffer);
336
                                        uart1_puts(";");
337
                                        itoa( Motor[m-1].State, buffer, 10);   //
338
                                        uart1_puts(buffer);
339
                                        uart1_puts(";");
340
                                        itoa( Motor[m-1].ReadMode, buffer, 10);   //
341
                                        uart1_puts(buffer);
342
                                        uart1_puts(";");
343
                                        itoa( Motor[m-1].Current, buffer, 10);   //
344
                                        uart1_puts(buffer);
345
                                        uart1_puts(";");
346
                                        itoa( Motor[m-1].MaxPWM, buffer, 10);   //
347
                                        uart1_puts(buffer);
348
                                        uart1_puts(";");
349
                                        itoa( Motor[m-1].Temperature, buffer, 10);   //
350
                                        uart1_puts(buffer);
351
                                        uart1_puts("\r");
352
                                        uart1_puts("\n");
353
                                        SerLoop =200;
354
                                }
355
                                else
356
                                        SerLoop--;
357
                        }
358
 
359
#endif
360
 
361
#if defined HWVERSION1_2W || defined HWVERSION1_2
362
 
363
                        if (Motor[m-1].SetPoint > 0)
364
                        {
365
                                if (SerLoop == 0)
366
                                {
367
                                        itoa( m-1, buffer, 10);   // convert interger into string (decimal format)
368
                                        USART_puts(buffer);        // and transmit string to UART
369
                                        USART_puts(";");
370
                                        itoa( Motor[m-1].Version, buffer, 10);   //
371
                                        USART_puts(buffer);
372
                                        USART_puts(";");
373
                                        itoa( Motor[m-1].SetPoint, buffer, 10);   //
374
                                        USART_puts(buffer);
375
                                        USART_puts(";");
376
                                        itoa( Motor[m-1].SetPointLowerBits, buffer, 10);   //
377
                                        USART_puts(buffer);
378
                                        USART_puts(";");
379
                                        itoa( Motor[m-1].State, buffer, 10);   //
380
                                        USART_puts(buffer);
381
                                        USART_puts(";");
382
                                        itoa( Motor[m-1].ReadMode, buffer, 10);   //
383
                                        USART_puts(buffer);
384
                                        USART_puts(";");
385
                                        itoa( Motor[m-1].Current, buffer, 10);   //
386
                                        USART_puts(buffer);
387
                                        USART_puts(";");
388
                                        itoa( Motor[m-1].MaxPWM, buffer, 10);   //
389
                                        USART_puts(buffer);
390
                                        USART_puts(";");
391
                                        itoa( Motor[m-1].Temperature, buffer, 10);   //
392
                                        USART_puts(buffer);
393
                                        USART_puts("\r");
394
                                        USART_puts("\n");
395
                                        SerLoop =200;
396
                                }
397
                                else
398
                                        SerLoop--;
399
                        }
400
 
401
#endif
402
 
403
                        while(!(BLFlags & BLFLAG_TX_COMPLETE)  && !CheckDelay(timer)); //wait for complete transfer
404
                }
405
                else
406
                        motor (m,v); //if (MotorMode == I2C_Mode)//
407
 
408
        }
409
        while (!get_key_press (1 << KEY_ESC));
410
 
411
        get_key_press(KEY_ALL);
412
 
413
        if (MotorMode == FC_Mode)
414
        {
415
                motor(0,0);     // switch all engines off at exit
416
        }
417
 
418
}
419