| 0,0 → 1,419 |
|---|
| /***************************************************************************** |
| * 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 |
| } |
| |
| } |
| |