Blame |
Last modification |
View Log
| RSS feed
/*****************************************************************************
* Copyright (C) 2008 Thomas Kaiser, thomas@ft-fanpage.de *
* Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net *
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net *
* Copyright (C) 2011 Harald Bongartz *
* *
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation; either version 2 of the License. *
* *
* This program is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU General Public License for more details. *
* *
* You should have received a copy of the GNU General Public License *
* along with this program; if not, write to the *
* Free Software Foundation, Inc., *
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
* *
* *
* Credits to: *
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN *
* http://www.mikrokopter.de *
* Gregor "killagreg" Stobrawa for his version of the MK code *
* Thomas Kaiser "thkais" for the original project. See *
* http://www.ft-fanpage.de/mikrokopter/ *
* http://forum.mikrokopter.de/topic-4061-1.html *
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code *
* http://www.mylifesucks.de/oss/c-osd/ *
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility*
*****************************************************************************/
#include "cpu.h"
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include <string.h>
#include <stdlib.h>
#include "main.h"
#include "motortest.h"
#include "lcd.h"
#include "timer.h"
#include "twimaster.h"
//#include "menu.h"
#include "uart1.h"
#include "usart.h"
#include "messages.h"
uint8_t m
;
uint8_t mmode
; // 0=Value 1=Motor
uint8_t v
;
volatile uint8_t i2c_state
;
volatile uint8_t motor_addr
= 0;
//--------------------------------------------------------------
// Senden der Motorwerte per I2C-Bus
//
void SendMotorData
(uint8_t m
,uint8_t v
)
{
if (m
==0)
for(m
=0;m
<MAX_MOTORS
;m
++) // alle Motoren
{
// Motor[m].SetPoint = MotorTest[m];
Motor
[m
].
SetPoint = v
;
Motor
[m
].
SetPointLowerBits = 0;
// Motor[i].SetPoint = MotorTest[i] / 4; // testing the high resolution
// Motor[i].SetPointLowerBits = MotorTest[i] % 4;
}
else
{
Motor
[m
-1].
SetPoint = v
;
Motor
[m
-1].
SetPointLowerBits = 0;
}
if(I2C_TransferActive
)
I2C_TransferActive
= 0; // enable for the next time
else
{
motor_write
= 0;
I2C_Start
(TWI_STATE_MOTOR_TX
); //Start I2C Interrupt Mode
}
}
//--------------------------------------------------------------
//
void Search_BL
(void)
{
uint8_t i
= 0;
unsigned int timer
;
lcd_cls
();
MotorenEin
=0;
MotorTest
[i
] = 0;
lcd_printp
(PSTR
("Suche BL-Ctrl"), 0);
// Check connected BL-Ctrls
BLFlags
|= BLFLAG_READ_VERSION
;
motor_read
= 0; // read the first I2C-Data
SendMotorData
(0,0);
timer
= SetDelay
(1);
while(!(BLFlags
& BLFLAG_TX_COMPLETE
) && !CheckDelay
(timer
)); //wait for complete transfer
timer
= SetDelay
(1);
for(i
=0; i
< MAX_MOTORS
; i
++)
{
SendMotorData
(i
,0);
while(!(BLFlags
& BLFLAG_TX_COMPLETE
) && !CheckDelay
(timer
)); //wait for complete transfer
if(Motor
[i
].
State & MOTOR_STATE_PRESENT_MASK
)
{
lcd_printp_at
(0, 1, PSTR
("Found BL-Ctrl:"), 0);
lcd_print_hex_at
(18,1,i
,0);
lcd_printp_at
(0, 2, PSTR
("Version:"), 0);
lcd_print_hex_at
(8,2,Motor
[i
].
Version,0);
lcd_printp_at
(11, 2, PSTR
("SetPoi:"), 0);
lcd_print_hex_at
(18,2,Motor
[i
].
SetPoint,0);
lcd_printp_at
(0, 3, PSTR
("SetPoiL:"), 0);
lcd_print_hex_at
(8,3,Motor
[i
].
SetPointLowerBits,0);
lcd_printp_at
(11, 3, PSTR
("State :"), 0);
lcd_print_hex_at
(18,3,Motor
[i
].
State,0);
lcd_printp_at
(0, 4, PSTR
("ReadMod:"), 0);
lcd_print_hex_at
(8,4,Motor
[i
].
ReadMode,0);
lcd_printp_at
(11, 4, PSTR
("Currnt:"), 0);
lcd_print_hex_at
(18,4,Motor
[i
].
Current,0);
lcd_printp_at
(0, 5, PSTR
("MaxPWM :"), 0);
lcd_print_hex_at
(8,5,Motor
[i
].
MaxPWM,0);
lcd_printp_at
(11, 5, PSTR
("Temp :"), 0);
write_ndigit_number_u
(18,5,Motor
[i
].
Temperature,3,1);
}
} //End For I
}
//--------------------------------------------------------------
//
void motor
(uint8_t m
,uint8_t v
)
{
memset (buffer
, 0, 16);
if(m
== 0)
{
memset (buffer
, v
, 16);
}
else
{
buffer
[m
-1] = v
;
}
SendOutData
('t', ADDRESS_FC
, 1, buffer
, 16);
}
//--------------------------------------------------------------
//
void motor_test
(uint8_t MotorMode
)
{
lcd_cls
();
mmode
= 1; // 1=Motor
m
= 1;
v
= 0;
char buffer
[7];
unsigned int SerLoop
;
SerLoop
= 10;
if (MotorMode
== I2C_Mode
)
{
Search_BL
();
do
{
lcd_printp_at
(11, 7, PSTR
("Ende Check"), 0);
if (get_key_press
(1 << KEY_ESC
))
{
get_key_press
(KEY_ALL
);
return;
}
}
while (!get_key_press
(1 << KEY_ENTER
));
}
lcd_cls
();
lcd_printp
(PSTR
(" BL-Ctrl Test "), 2);
lcd_printp_at
(2, 2, PSTR
("Motor: 1"), 0);
lcd_printp_at
(2, 3, PSTR
("Value: 0"), 0);
lcd_frect
((8*1), (8*5), (0 * (14*8)) / 255, 6, 1);
// lcd_printp_at (0, 7, PSTR(KEY_LINE_3), 0);
lcd_puts_at
(0, 7, strGet
(KEYLINE3
), 0);
lcd_printp_at
(18, 7, PSTR
("\x1a \x1b"), 0);
lcd_printp_at
(0, 2, PSTR
("\x1d"), 0);
#if defined HWVERSION1_3W || defined HWVERSION1_3 || defined HWVERSION3_9
if (MotorMode
== I2C_Mode
)
uart1_puts
("Motor;Version;Setpoint high;Setpoint low;State;ReadMode;Current;MaxPWM;Temperature\r");
#endif
#if defined HWVERSION1_2W || defined HWVERSION1_2
if (MotorMode
== I2C_Mode
)
USART_puts
("Motor;Version;Setpoint high;Setpoint low;State;ReadMode;Current;MaxPWM;Temperature\r");
#endif
if (MotorMode
== FC_Mode
)
{
if (hardware
== NC
&& current_hardware
== NC
)
{
SwitchToFC
();
}
}
do
{
// mmode 0=Value 1=Motor
if ((mmode
== 0) && (get_key_press
(1 << KEY_PLUS
) || get_key_long_rpt_sp
((1 << KEY_PLUS
), 3)) && (v
< 254))
{
v
++;
write_ndigit_number_u
(9, 3, v
, 3, 0);
if (MotorMode
== FC_Mode
)
lcd_frect
((8*1), (8*5), (v
* (14*8)) / 255, 6, 1);
}
if ((mmode
== 0) && (get_key_press
(1 << KEY_MINUS
) || get_key_long_rpt_sp
((1 << KEY_MINUS
), 3)) && (v
> 0))
{
if (MotorMode
== FC_Mode
)
lcd_frect
(((v
* (14*8) / 255) + 8), (8*5), ((14*8) / 255), 6, 0);
v
--;
write_ndigit_number_u
(9, 3, v
, 3, 0);
if (MotorMode
== FC_Mode
)
lcd_frect
((8*1), (8*5), (v
* (14*8)) / 255, 6, 1);
}
if ((mmode
== 1) && (get_key_press
(1 << KEY_PLUS
) || get_key_long_rpt_sp
((1 << KEY_PLUS
), 1)) && (m
< 16))
{
m
++;
write_ndigit_number_u
(9, 2, m
, 3, 0);
}
if ((mmode
== 1) && (get_key_press
(1 << KEY_MINUS
) || get_key_long_rpt_sp
((1 << KEY_MINUS
), 1)) && (m
> 0))
{
m
--;
if(m
> 0)
write_ndigit_number_u
(9, 2, m
, 3, 0);
if(m
== 0)
lcd_printp_at
(9, 2, PSTR
("All"), 0);
}
if (get_key_press
(1 << KEY_ENTER
))
{
if (MotorMode
== I2C_Mode
)
{
if (v
> 0)
{
m
= 0;
v
=0;
lcd_frect
((8*1), (8*5), (0 * (14*8)) / 255, 6, 1);
lcd_cls_line
(0, 5, 21);
if(m
> 0) write_ndigit_number_u
(9, 2, m
, 3, 0);
if(m
== 0) lcd_printp_at
(9, 2, PSTR
("All"), 0);
write_ndigit_number_u
(9, 3, v
, 3, 0);
SendMotorData
(m
,v
);
timer
= SetDelay
(1);
while(!(BLFlags
& BLFLAG_TX_COMPLETE
) && !CheckDelay
(timer
)); //wait for complete transfer
}
}
if(mmode
== 0) // 0=Value
{
lcd_printp_at
(0, 2, PSTR
("\x1d"), 0);
lcd_printp_at
(0, 3, PSTR
(" "), 0);
mmode
= 1; // 1=Motor
}
else
{
lcd_printp_at
(0, 2, PSTR
(" "), 0);
lcd_printp_at
(0, 3, PSTR
("\x1d"), 0);
mmode
= 0; // 0=Value
}
}
//if (get_key_press (1 << KEY_ENTER))//
if (MotorMode
== I2C_Mode
)
{
SendMotorData
(m
,v
);
timer
= SetDelay
(1);
lcd_printp_at
(0, 3, PSTR
("SetPoint :"), 0);
write_ndigit_number_u
(13,3,Motor
[m
-1].
SetPoint,3,0);
lcd_printp_at
(0, 4, PSTR
("Current :"), 0);
lcd_print_hex_at
(13,4,Motor
[m
-1].
Current,0);
write_ndigit_number_u
(13,4,Motor
[m
-1].
Current,3,0);
lcd_printp_at
(0, 5, PSTR
("Temperature:"), 0);
write_ndigit_number_u
(13,5,Motor
[m
-1].
Temperature,3,0);
lcd_printp_at
(0, 6, PSTR
("Version:"), 0);
lcd_print_hex_at
(8,6,Motor
[m
-1].
Version,0);
lcd_printp_at
(11, 6, PSTR
("State :"), 0);
lcd_print_hex_at
(18,6,Motor
[m
-1].
State,0);
#if defined HWVERSION1_3W || defined HWVERSION1_3 || defined HWVERSION3_9
if (Motor
[m
-1].
SetPoint > 0)
{
if (SerLoop
== 0)
{
itoa( m
-1, buffer
, 10); // convert interger into string (decimal format)
uart1_puts
(buffer
); // and transmit string to UART
uart1_puts
(";");
itoa( Motor
[m
-1].
Version, buffer
, 10); //
uart1_puts
(buffer
);
uart1_puts
(";");
itoa( Motor
[m
-1].
SetPoint, buffer
, 10); //
uart1_puts
(buffer
);
uart1_puts
(";");
itoa( Motor
[m
-1].
SetPointLowerBits, buffer
, 10); //
uart1_puts
(buffer
);
uart1_puts
(";");
itoa( Motor
[m
-1].
State, buffer
, 10); //
uart1_puts
(buffer
);
uart1_puts
(";");
itoa( Motor
[m
-1].
ReadMode, buffer
, 10); //
uart1_puts
(buffer
);
uart1_puts
(";");
itoa( Motor
[m
-1].
Current, buffer
, 10); //
uart1_puts
(buffer
);
uart1_puts
(";");
itoa( Motor
[m
-1].
MaxPWM, buffer
, 10); //
uart1_puts
(buffer
);
uart1_puts
(";");
itoa( Motor
[m
-1].
Temperature, buffer
, 10); //
uart1_puts
(buffer
);
uart1_puts
("\r");
uart1_puts
("\n");
SerLoop
=200;
}
else
SerLoop
--;
}
#endif
#if defined HWVERSION1_2W || defined HWVERSION1_2
if (Motor
[m
-1].
SetPoint > 0)
{
if (SerLoop
== 0)
{
itoa( m
-1, buffer
, 10); // convert interger into string (decimal format)
USART_puts
(buffer
); // and transmit string to UART
USART_puts
(";");
itoa( Motor
[m
-1].
Version, buffer
, 10); //
USART_puts
(buffer
);
USART_puts
(";");
itoa( Motor
[m
-1].
SetPoint, buffer
, 10); //
USART_puts
(buffer
);
USART_puts
(";");
itoa( Motor
[m
-1].
SetPointLowerBits, buffer
, 10); //
USART_puts
(buffer
);
USART_puts
(";");
itoa( Motor
[m
-1].
State, buffer
, 10); //
USART_puts
(buffer
);
USART_puts
(";");
itoa( Motor
[m
-1].
ReadMode, buffer
, 10); //
USART_puts
(buffer
);
USART_puts
(";");
itoa( Motor
[m
-1].
Current, buffer
, 10); //
USART_puts
(buffer
);
USART_puts
(";");
itoa( Motor
[m
-1].
MaxPWM, buffer
, 10); //
USART_puts
(buffer
);
USART_puts
(";");
itoa( Motor
[m
-1].
Temperature, buffer
, 10); //
USART_puts
(buffer
);
USART_puts
("\r");
USART_puts
("\n");
SerLoop
=200;
}
else
SerLoop
--;
}
#endif
while(!(BLFlags
& BLFLAG_TX_COMPLETE
) && !CheckDelay
(timer
)); //wait for complete transfer
}
else
motor
(m
,v
); //if (MotorMode == I2C_Mode)//
}
while (!get_key_press
(1 << KEY_ESC
));
get_key_press
(KEY_ALL
);
if (MotorMode
== FC_Mode
)
{
motor
(0,0); // switch all engines off at exit
}
}