Rev 1544 |
Blame |
Compare with Previous |
Last modification |
View Log
| RSS feed
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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
= ATMEGA644P
;
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
#ifdef USE_RC_JENNIC // LPD: FC_JN_Receiver support added
printf("\r\nSupport for JENNIC 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);
}