| 0,0 → 1,453 |
|---|
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Copyright (c) Holger Buss, Ingo Busker |
| // + Nur für den privaten Gebrauch |
| // + www.MikroKopter.com |
| // + porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), |
| // + dass eine Nutzung (auch auszugsweise) nur für den privaten und nicht-kommerziellen Gebrauch zulässig ist. |
| // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
| // + bzgl. der Nutzungsbedingungen aufzunehmen. |
| // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen, |
| // + Verkauf von Luftbildaufnahmen, usw. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, |
| // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
| // + auf anderen Webseiten oder Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
| // + eindeutig als Ursprung verlinkt und genannt werden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
| // + Benutzung auf eigene Gefahr |
| // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
| // + mit unserer Zustimmung zulässig |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
| // + this list of conditions and the following disclaimer. |
| // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
| // + from this software without specific prior written permission. |
| // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
| // + for non-commercial use (directly or indirectly) |
| // + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
| // + with our written permission |
| // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
| // + clearly linked as origin |
| // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
| // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
| // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
| // + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| // + POSSIBILITY OF SUCH DAMAGE. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #include <avr/boot.h> |
| |
| #include <avr/io.h> |
| #include <avr/interrupt.h> |
| #include <util/delay.h> |
| |
| #include "main.h" |
| #include "timer0.h" |
| #include "timer2.h" |
| #include "uart0.h" |
| #include "uart1.h" |
| #include "led.h" |
| #include "menu.h" |
| #include "fc.h" |
| #include "rc.h" |
| #include "analog.h" |
| #include "printf_P.h" |
| #ifdef USE_KILLAGREG |
| #include "mm3.h" |
| #endif |
| #ifdef USE_NAVICTRL |
| #include "spi.h" |
| #endif |
| #ifdef USE_MK3MAG |
| #include "mk3mag.h" |
| #endif |
| #include "twimaster.h" |
| #include "eeprom.h" |
| |
| |
| uint8_t BoardRelease = 10; |
| uint8_t CPUType = ATMEGA644; |
| uint8_t LowVoltageWarning = 94; |
| uint16_t FlightMinutes = 0, FlightMinutesTotal = 0; |
| |
| uint8_t GetCPUType(void) |
| { // works only after reset or power on when the registers have default values |
| uint8_t CPUType = ATMEGA644; |
| if( (UCSR1A == 0x20) && (UCSR1C == 0x06) ) CPUType = ATMEGA644P; // initial Values for 644P after reset |
| return CPUType; |
| } |
| |
| |
| uint8_t GetBoardRelease(void) |
| { |
| uint8_t BoardRelease = 10; |
| // the board release is coded via the pull up or down the 2 status LED |
| |
| PORTB &= ~((1 << PORTB1)|(1 << PORTB0)); // set tristate |
| DDRB &= ~((1 << DDB0)|(1 << DDB0)); // set port direction as input |
| |
| _delay_loop_2(1000); // make some delay |
| |
| switch( PINB & ((1<<PINB1)|(1<<PINB0)) ) |
| { |
| case 0x00: |
| BoardRelease = 10; // 1.0 |
| break; |
| case 0x01: |
| BoardRelease = 11; // 1.1 or 1.2 |
| break; |
| case 0x02: |
| BoardRelease = 20; // 2.0 |
| break; |
| case 0x03: |
| BoardRelease = 13; // 1.3 |
| break; |
| default: |
| break; |
| } |
| // set LED ports as output |
| DDRB |= (1<<DDB1)|(1<<DDB0); |
| RED_ON; |
| GRN_OFF; |
| return BoardRelease; |
| } |
| |
| void LipoDetection(uint8_t print) |
| { |
| #define MIN_VOLTAGE_WARNING 93 // minimum low voltage warning to protect LiPO |
| uint16_t timer; |
| if(print) printf("\n\rBatt:"); |
| if(ParamSet.LowVoltageWarning < 50) // below 5.0 V the value is interpreted as single cell low volatage warning |
| { |
| timer = SetDelay(500); // wait at least 500 ms to get stable adc readings |
| if(print) while(!CheckDelay(timer)); |
| if(UBat < 130) // less than 13.0 V must be a 3S Lipo |
| { |
| LowVoltageWarning = 3 * ParamSet.LowVoltageWarning; |
| if(print) |
| { |
| Beep(3, 200); |
| printf(" 3 Cells "); |
| } |
| } |
| else // <= 13.0V |
| { |
| LowVoltageWarning = 4 * ParamSet.LowVoltageWarning; |
| if(print) |
| { |
| Beep(4, 200); |
| printf(" 4 Cells "); |
| } |
| } |
| } |
| else // classic settings |
| { |
| LowVoltageWarning = ParamSet.LowVoltageWarning; |
| } |
| //if(LowVoltageWarning < MIN_VOLTAGE_WARNING) LowVoltageWarning = MIN_VOLTAGE_WARNING; |
| if(print) printf("Low Warning level: %d.%dV", LowVoltageWarning/10, LowVoltageWarning%10); |
| } |
| |
| int16_t main (void) |
| { |
| uint16_t timer, flighttimer; |
| uint8_t i; |
| |
| // disable interrupts global |
| cli(); |
| |
| // analyze hardware environment |
| CPUType = GetCPUType(); |
| BoardRelease = GetBoardRelease(); |
| |
| // disable watchdog |
| MCUSR &=~(1<<WDRF); |
| WDTCSR |= (1<<WDCE)|(1<<WDE); |
| WDTCSR = 0; |
| |
| BeepTime = 2000; |
| |
| PPM_in[CH_GAS] = 0; |
| StickYaw = 0; |
| StickRoll = 0; |
| StickNick = 0; |
| |
| RED_OFF; |
| |
| // initalize modules |
| LED_Init(); |
| TIMER0_Init(); |
| TIMER2_Init(); |
| USART0_Init(); |
| if(CPUType == ATMEGA644P) USART1_Init(); |
| RC_Init(); |
| ADC_Init(); |
| I2C_Init(); |
| #ifdef USE_NAVICTRL |
| SPI_MasterInit(); |
| #endif |
| #ifdef USE_KILLAGREG |
| MM3_Init(); |
| #endif |
| #ifdef USE_MK3MAG |
| MK3MAG_Init(); |
| #endif |
| |
| // enable interrupts global |
| sei(); |
| |
| printf("\n\r==================================="); |
| printf("\n\rFlightControl"); |
| printf("\n\rHardware: %d.%d", BoardRelease/10, BoardRelease%10); |
| printf("\n\rthe use of this software is only permitted \n\ron original MikroKopter-Hardware"); |
| printf("\n\rwww.MikroKopter.de (c) HiSystems GmbH"); |
| printf("\n\r==================================="); |
| |
| if(CPUType == ATMEGA644P) |
| printf("\r\n CPU: Atmega644p"); |
| else |
| printf("\r\n CPU: Atmega644"); |
| printf("\n\rSoftware: V%d.%d%c",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a'); |
| printf("\n\r==================================="); |
| GRN_ON; |
| |
| // Parameter Set handling |
| ParamSet_Init(); |
| |
| // Check connected BL-Ctrls |
| printf("\n\rFound BL-Ctrl: "); |
| motor_read = 0; |
| UpdateMotor = 0; |
| SendMotorData(); |
| while(!UpdateMotor); |
| motor_read = 0; // read the first I2C-Data |
| timer = SetDelay(2000); // set timeout to 2 seconds |
| for(i = 0; i < MAX_MOTORS; i++) |
| { |
| UpdateMotor = 0; |
| SendMotorData(); |
| while(!UpdateMotor); // wait 2 ms to finish data transfer |
| if(Mixer.Motor[i][MIX_GAS] > 0) // wait max 2 sec for the BL-Ctrls to wake up |
| { |
| while(!CheckDelay(timer) && !(Motor[i].Present) ) // while not timeout and motor is not present |
| { |
| UpdateMotor = 0; |
| SendMotorData(); |
| while(!UpdateMotor); // wait 2 ms |
| } |
| } |
| if(Motor[i].Present) printf("%d ",i+1); |
| } |
| for(i = 0; i < MAX_MOTORS; i++) |
| { |
| if(!Motor[i].Present && Mixer.Motor[i][MIX_GAS] > 0) |
| { |
| printf("\n\r\n\r!! MISSING BL-CTRL: %d !!",i+1); |
| Servo_On(); // just in case the FC would be used as camera-stabilizer |
| } |
| Motor[i].Error = 0; |
| } |
| printf("\n\r==================================="); |
| SendMotorData(); |
| |
| //wait for a short time (otherwise the RC channel check won't work below) |
| timer = SetDelay(500); |
| while(!CheckDelay(timer)); |
| |
| if(ParamSet.Config0 & CFG0_AIRPRESS_SENSOR) |
| { |
| printf("\n\rCalibrating air pressure sensor.."); |
| timer = SetDelay(1000); |
| SearchAirPressureOffset(); |
| while (!CheckDelay(timer)); |
| printf("OK\n\r"); |
| } |
| |
| #ifdef USE_NAVICTRL |
| printf("\n\rSupport for NaviCtrl"); |
| #ifdef USE_RC_DSL |
| printf("\r\nSupport for DSL RC at 2nd UART"); |
| #endif |
| #ifdef USE_RC_SPECTRUM |
| printf("\r\nSupport for SPEKTRUM RC at 2nd UART"); |
| #endif |
| #endif |
| |
| #ifdef USE_KILLAGREG |
| printf("\n\rSupport for MicroMag3 Compass"); |
| #endif |
| |
| #ifdef USE_MK3MAG |
| printf("\n\rSupport for MK3MAG Compass"); |
| #endif |
| |
| #if (defined (USE_KILLAGREG) || defined (USE_MK3MAG)) |
| if(CPUType == ATMEGA644P) printf("\n\rSupport for GPS at 2nd UART"); |
| else printf("\n\rSupport for GPS at 1st UART"); |
| #endif |
| |
| // init variables |
| SetNeutral(NO_ACC_CALIB); |
| |
| RED_OFF; |
| |
| BeepTime = 2000; |
| ExternControl.Digital[0] = 0x55; |
| |
| |
| FlightMinutes = GetParamWord(PID_FLIGHT_MINUTES); |
| FlightMinutesTotal = GetParamWord(PID_FLIGHT_MINUTES_TOTAL); |
| flighttimer = 0; |
| if( (FlightMinutesTotal == 0xFFFF) || (FlightMinutes == 0xFFFF) ) |
| { |
| FlightMinutes = 0; |
| FlightMinutesTotal = 0; |
| } |
| printf("\n\rFlight-time %u min Total:%u min", FlightMinutes, FlightMinutesTotal); |
| |
| |
| printf("\n\rControl: "); |
| if (ParamSet.Config0 & CFG0_HEADING_HOLD) printf("HeadingHold"); |
| else printf("Neutral (ACC-Mode)"); |
| |
| LCD_Clear(); |
| I2CTimeout = 5000; |
| LipoDetection(1); |
| printf("\n\r===================================\n\r"); |
| |
| timer = SetDelay(2000); |
| |
| // begin of main loop |
| while (1) |
| { |
| if(UpdateMotor && ADReady) // control interval |
| { |
| UpdateMotor = 0; // reset Flag, is enabled every 2 ms by ISR of timer0 |
| |
| //J4HIGH; |
| MotorControl(); |
| //J4LOW; |
| |
| SendMotorData(); // the flight control code |
| RED_OFF; |
| |
| if(RC_Quality) RC_Quality--; |
| else PPM_INPUT_ON; // if RC-Quality is lost, enable PPM input (could be disabled by a receiver on uart1) |
| |
| #ifdef USE_NAVICTRL |
| if(NCDataOkay) |
| { |
| if(--NCDataOkay == 0) // no data from NC |
| { // set gps control sticks neutral |
| GPSStickNick = 0; |
| GPSStickRoll = 0; |
| NCSerialDataOkay = 0; |
| NCGpsZ = 0; |
| } |
| } |
| #endif |
| |
| if(!--I2CTimeout || MissingMotor) // try to reset the i2c if motor is missing ot timeout |
| { |
| RED_ON; |
| if(!I2CTimeout) |
| { |
| I2C_Reset(); |
| I2CTimeout = 5; |
| DebugOut.Analog[28]++; // I2C-Error |
| } |
| if((BeepModulation == 0xFFFF) && (MKFlags & MKFLAG_MOTOR_RUN) ) |
| { |
| BeepTime = 10000; // 1 second |
| BeepModulation = 0x0080; |
| } |
| } |
| else |
| { |
| RED_OFF; |
| } |
| |
| // allow Serial Data Transmit if motors must not updated or motors are not running |
| if( !UpdateMotor || !(MKFlags & MKFLAG_MOTOR_RUN) ) |
| { |
| USART0_TransmitTxData(); |
| } |
| USART0_ProcessRxData(); |
| |
| if(CheckDelay(timer)) |
| { |
| timer += 20; // every 20 ms |
| if(PcAccess) PcAccess--; |
| else |
| { |
| ExternControl.Config = 0; |
| ExternStickNick= 0; |
| ExternStickRoll = 0; |
| ExternStickYaw = 0; |
| if((BeepModulation == 0xFFFF) && (RC_Quality == 0)) |
| { |
| BeepTime = 15000; // 1.5 seconds |
| BeepModulation = 0x0C00; |
| } |
| } |
| if(UBat < LowVoltageWarning) |
| { |
| MKFlags |= MKFLAG_LOWBAT; |
| BeepModulation = 0x0300; |
| if(!BeepTime ) |
| { |
| BeepTime = 6000; // 0.6 seconds |
| } |
| } |
| else |
| { |
| MKFlags &= ~MKFLAG_LOWBAT; |
| } |
| #ifdef USE_NAVICTRL |
| SPI_StartTransmitPacket(); |
| SendSPI = 4; |
| #endif |
| |
| if(!(MKFlags & MKFLAG_MOTOR_RUN)) flighttimer = 1450; // 0.5 minutes round up |
| if(++flighttimer == 2930) // one minute |
| { |
| flighttimer = 0; |
| FlightMinutesTotal++; |
| FlightMinutes++; |
| SetParamWord(PID_FLIGHT_MINUTES_TOTAL, FlightMinutesTotal); |
| SetParamWord(PID_FLIGHT_MINUTES, FlightMinutes); |
| timer = SetDelay(20); // in case "timer += 20;" will not work |
| } |
| }// EOF CheckDelay(timer) |
| |
| LED_Update(); |
| } |
| |
| #ifdef USE_NAVICTRL |
| if(!SendSPI) |
| { // SendSPI is decremented in timer0.c with a rate of 9.765 kHz. |
| // within the SPI_TransmitByte() routine the value is set to 4. |
| // I.e. the SPI_TransmitByte() is called at a rate of 9.765 kHz/4= 2441.25 Hz, |
| // and therefore the time of transmission of a complete spi-packet (32 bytes) is 32*4/9.765 kHz = 13.1 ms. |
| SPI_TransmitByte(); |
| } |
| #endif |
| } |
| return (1); |
| } |
| |