Rev 2248 |
Blame |
Compare with Previous |
Last modification |
View Log
| RSS feed
/**************************************************************************************************************************************
* File: main.c
*
* Purpose: main function fot Flight Ctrl
*
* Functions: void CalMk3Mag(void)
* void LipoDetection(unsigned char print)
* int main(void)
*
* hardware: Flight Ctrl V1.3
*
* Created: Feb 2013
*
* Revisions: 1.00 experimental subversion for a balancekopter
* Achtung: nicht flugfähige Experimentalversion für eine Balenwaage mit Flight-CTRL
* siehe: http://forum.mikrokopter.de/topic-39231.html
*
* Copyright: (c)2013 www.mikrokopter.de
* All rights reserved. This software is available only for non-commercial or educational applications.
* Other uses are prohibited. This software may be modified only if
* the resulting code be made available publicly and the original author(s) given credit.
*
************************************************************************************************************************************/
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) Holger Buss, Ingo Busker
// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY
// + www.MikroKopter.com
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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 oder Nutzung 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 the sources to other systems or using the software on other systems (except 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 "main.h"
unsigned char PlatinenVersion
= 13;
unsigned char SendVersionToNavi
= 1;
unsigned char BattLowVoltageWarning
= 94;
unsigned int FlugMinuten
= 0,FlugMinutenGesamt
= 0;
//-----------------------------------------------------------------------------------------------------
// calibrate magnetic compass
//-----------------------------------------------------------------------------------------------------
void CalMk3Mag
(void)
{
static unsigned char stick
= 1;
if(PPM_in
[EE_Parameter.
Kanalbelegung[K_NICK
]] > -20) stick
= 0;
if((PPM_in
[EE_Parameter.
Kanalbelegung[K_NICK
]] < -70) && !stick
)
{
stick
= 1;
WinkelOut.
CalcState++;
if(WinkelOut.
CalcState > 4)
{
beeptime
= 1000;
}
else Piep
(WinkelOut.
CalcState,150);
}
DebugOut.
Analog[19] = WinkelOut.
CalcState;
}
//-----------------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------------
// recognize the LiPo accumulators
// char print can be 0 or 1
//-----------------------------------------------------------------------------------------------------
void LipoDetection
(unsigned char print
)
{
unsigned int timer
;
if(print
) printf("\n\rBatt:");
if(EE_Parameter.
UnterspannungsWarnung < 50) // automatic recognition of lipo cells -> default = 33
{
timer
= SetDelay
(500); //
if(print
) while (!CheckDelay
(timer
));
if(UBat
< 130)
{
BattLowVoltageWarning
= 3 * EE_Parameter.
UnterspannungsWarnung;
if(print
)
{
Piep
(3,200);
printf(" 3 Cells ");
}
}
else
{
BattLowVoltageWarning
= 4 * EE_Parameter.
UnterspannungsWarnung;
if(print
)
{
Piep
(4,200);
printf(" 4 Cells ");
}
}
}
else BattLowVoltageWarning
= EE_Parameter.
UnterspannungsWarnung;
if(print
) printf(" Low warning level: %d.%d",BattLowVoltageWarning
/10,BattLowVoltageWarning
%10);
}
//-----------------------------------------------------------------------------------------------------
// --------------------------------------------------------------------------------------------------------------------------
// main program starting here
//
// INPUT: None
// OUTPUT: None
// RETURN: 1
// --------------------------------------------------------------------------------------------------------------------------
int main
(void)
{
unsigned int timer
,i
,timer2
= 0;
DDRB
= 0x00;
PORTB
= 0x00;
for(timer
= 0; timer
< 1000; timer
++); // verzögern
PlatinenVersion
= 13;
DDRC
= 0x81; // SCL
PORTC
= 0xFF; // Pullup SDA
DDRB
= 0x1B; // LEDs und Druckoffset
PORTB
= 0x01; // LED_Rot
DDRD
= 0x3E; // Speaker & TXD & J3 J4 J5
PORTD
= 0x47; // LED
HEF4017R_OFF
; // #define HEF4017R_OFF PORTC &= ~(1<<PORTC6)
MCUSR
&=~
(1<<WDRF
); // MCUSR MCU Status Register provides information on which reset source caused an MCU reset
WDTCSR
|= (1<<WDCE
)|(1<<WDE
); // WDTCSR Watchdog Timer Control Register
WDTCSR
= 0;
beeptime
= 2000;
StickGier
= 0; PPM_in
[K_GAS
] = 0; StickRoll
= 0; StickNick
= 0;
GIER_GRAD_FAKTOR
= 1291;
ROT_OFF
; // rote LED aus // PORTB |= 0x01;
Timer_Init
(); // goto timer0.c Zeile 40
TIMER2_Init
(); // goto timer0.c Zeile 170
UART_Init
(); // goto uart.c Zeile 488
rc_sum_init
(); // goto rc.c line 17
ADC_Init
(); // goto analog.c Zeile 26
i2c_init
(); // goto twimaster.c Zeile 16
//---------------------------------------------------------------------------------------------------------------------------------
// passes one time through the following code before arriving at the forever loop
//---------------------------------------------------------------------------------------------------------------------------------
sei
();
printf("\n\r===================================");
printf("\n\rFlightControl\n\rHardware:%d.%d\n\rSoftware:V%d.%d%c ",PlatinenVersion
/10,PlatinenVersion
%10, VERSION_MAJOR
, VERSION_MINOR
,VERSION_PATCH
+ 'a');
printf("\n\rexperimental version");
printf("\n\r===================================");
GRN_ON
; // switch green LED on // PORTB &=~0x02;
ReadParameterSet
(3, (unsigned char *) &EE_Parameter.
Kanalbelegung[0], 9); // read first 9 Bytes = chanal setup of radio control
// ---------------------------------------------------------------------------------------------------------------------------
// setup of mixer
// ---------------------------------------------------------------------------------------------------------------------------
// check MIXER-Revision at first EEPROM Byte = 1000 // #define EEPROM_ADR_MIXER_TABLE 1000
// if((eeprom_read_byte(&EEPromArray[EEPROM_ADR_MIXER_TABLE]) == MIXER_REVISION) && (eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) != 0xff))
if((eeprom_read_byte
(&EEPromArray
[1000]) == 1) && (eeprom_read_byte
(&EEPromArray
[1]) != 0xFF)) // aufgelöst
{
unsigned char i
; // Settings via Koptertool zurücksetzen
RequiredMotors
= 0;
eeprom_read_block
(&Mixer
, &EEPromArray
[EEPROM_ADR_MIXER_TABLE
], sizeof(Mixer
));
for(i
=0; i
<16;i
++) { if(Mixer.
Motor[i
][0] > 0) RequiredMotors
++;}
}
else // default
{
unsigned char i
;
printf("\n\rerzeugt default Mixer Table");
for(i
=0; i
<16;i
++) { Mixer.
Motor[i
][0] = 0;Mixer.
Motor[i
][1] = 0;Mixer.
Motor[i
][2] = 0;Mixer.
Motor[i
][3] = 0;};
// default = Quadro
// number 64 is equivalent to 100%
// GasMischanteil pd_ergebnis_nick pd_ergebnis_roll GierMischanteil
Mixer.
Motor[0][0] = 64; Mixer.
Motor[0][1] = +64; Mixer.
Motor[0][2] = 0; Mixer.
Motor[0][3] = +64; // vorne
Mixer.
Motor[1][0] = 64; Mixer.
Motor[1][1] = -64; Mixer.
Motor[1][2] = 0; Mixer.
Motor[1][3] = +64; // hinten
Mixer.
Motor[2][0] = 64; Mixer.
Motor[2][1] = 0; Mixer.
Motor[2][2] = -64; Mixer.
Motor[2][3] = -64; // rechts
Mixer.
Motor[3][0] = 64; Mixer.
Motor[3][1] = 0; Mixer.
Motor[3][2] = +64; Mixer.
Motor[3][3] = -64; // links
Mixer.
Revision = MIXER_REVISION
; // #define MIXER_REVISION 1
memcpy(Mixer.
Name, "Quadro\0", 11);
eeprom_write_block
(&Mixer
, &EEPromArray
[EEPROM_ADR_MIXER_TABLE
], sizeof(Mixer
)); // sizeof(Mixer) = 77
}
printf("\n\rMixer-Config: '%s' (%u Motors)",Mixer.
Name,RequiredMotors
);
// ---------------------------------------------------------------------------------------------------------------------------
// how many BL-Ctrls are connected ?
// ---------------------------------------------------------------------------------------------------------------------------
printf("\n\r...BL-Ctrl....");
motorread
= 0;
UpdateMotor
= 0;
SendMotorData
();
while(!UpdateMotor
);
motorread
= 0; // read the first I2C-Datasets
timer
= SetDelay
(2000); // sets
for(i
=0; i
< MAX_MOTORS
; i
++) // #define MAX_MOTORS 4
{
UpdateMotor
= 0;
SendMotorData
(); // goto fc.c line 460
while(!UpdateMotor
);
if(Mixer.
Motor[i
][0] > 0) // wait maximum 2 sec to wake up the BL-Ctrls
{
while(!CheckDelay
(timer
) && !MotorPresent
[i
])
{
UpdateMotor
= 0;
SendMotorData
();
while(!UpdateMotor
);
}
}
if(MotorPresent
[i
]) printf("%d ",i
+1);
}
for(i
=0; i
< MAX_MOTORS
; i
++)
{
if(!MotorPresent
[i
] && Mixer.
Motor[i
][0] > 0) // #define MAX_MOTORS 4
{
printf("\n\r\n\r!! missing BL-CTRL: %d !!",i
+1);
ServoActive
= 1; // just in case the FlightCtrl would be used as camera-stabilizer
}
MotorError
[i
] = 0;
}
printf("\n\r===================================");
SendMotorData
();
// ---------------------------------------------------------------------------------------------------------------------------
// check, that the revision in EEPROM fits to actual software
// ---------------------------------------------------------------------------------------------------------------------------
if(eeprom_read_byte
(&EEPromArray
[EEPROM_ADR_VALID
]) != EE_DATENREVISION
) // #define EEPROM_ADR_VALID 1
{ // #define EE_DATENREVISION 80
DefaultKonstanten1
(); // Funktion aus eeprom.c
printf("\n\rInit. EEPROM");
for (unsigned char i
=1;i
<6;i
++) // es gibt 5 verschiedene Settings
{
if(i
==2) DefaultKonstanten2
(); // Kamera
if(i
==3) DefaultKonstanten3
(); // Anfänger
if(i
>3) DefaultKonstanten2
(); // Kamera
// ---------------------------------------------------------------------------------------------------------------------------
// valid Setting ?
// ---------------------------------------------------------------------------------------------------------------------------
if(eeprom_read_byte
(&EEPromArray
[EEPROM_ADR_CHANNELS
]) < 12 && eeprom_read_byte
(&EEPromArray
[EEPROM_ADR_CHANNELS
+1]) < 12 && eeprom_read_byte
(&EEPromArray
[EEPROM_ADR_CHANNELS
+2]) < 12 && eeprom_read_byte
(&EEPromArray
[EEPROM_ADR_CHANNELS
+3]) < 12)
{
EE_Parameter.
Kanalbelegung[0] = eeprom_read_byte
(&EEPromArray
[EEPROM_ADR_CHANNELS
+0]); // Nick // #define EEPROM_ADR_CHANNELS 80
EE_Parameter.
Kanalbelegung[1] = eeprom_read_byte
(&EEPromArray
[EEPROM_ADR_CHANNELS
+1]); // Roll
EE_Parameter.
Kanalbelegung[2] = eeprom_read_byte
(&EEPromArray
[EEPROM_ADR_CHANNELS
+2]); // Gas
EE_Parameter.
Kanalbelegung[3] = eeprom_read_byte
(&EEPromArray
[EEPROM_ADR_CHANNELS
+3]); // Gier
EE_Parameter.
Kanalbelegung[4] = eeprom_read_byte
(&EEPromArray
[EEPROM_ADR_CHANNELS
+4]); // Poti 1
EE_Parameter.
Kanalbelegung[5] = eeprom_read_byte
(&EEPromArray
[EEPROM_ADR_CHANNELS
+5]); // Poti 2
EE_Parameter.
Kanalbelegung[6] = eeprom_read_byte
(&EEPromArray
[EEPROM_ADR_CHANNELS
+6]); // Poti 3
EE_Parameter.
Kanalbelegung[7] = eeprom_read_byte
(&EEPromArray
[EEPROM_ADR_CHANNELS
+7]); // Poti 4
if(i
==1) printf(": Generating Default-Parameter using old Stick Settings");
} else DefaultStickMapping
();
WriteParameterSet
(i
, (unsigned char *) &EE_Parameter.
Kanalbelegung[0], STRUCT_PARAM_LAENGE
); // #define STRUCT_PARAM_LAENGE sizeof(EE_Parameter)
// WriteParameterSet(i, (unsigned char *) &EE_Parameter.Kanalbelegung[0], 101));
}
SetActiveParamSetNumber
(3); // default Setting ist Beginner
eeprom_write_byte
(&EEPromArray
[EEPROM_ADR_VALID
], EE_DATENREVISION
); // #define EE_DATENREVISION 80
// eeprom_write_byte(&EEPromArray[1], 80); // aufgelöst
} // EOF : check existing revision at EEPROM
FlugMinuten
= (int)eeprom_read_byte
(&EEPromArray
[EEPROM_ADR_MINUTES2
]) * 256 + (int)eeprom_read_byte
(&EEPromArray
[EEPROM_ADR_MINUTES2
+1]);
FlugMinutenGesamt
= (int)eeprom_read_byte
(&EEPromArray
[EEPROM_ADR_MINUTES
]) * 256 + (int)eeprom_read_byte
(&EEPromArray
[EEPROM_ADR_MINUTES
+1]);
if(FlugMinutenGesamt
== 0xFFFF || FlugMinuten
== 0xFFFF) // Flugminuten sind am überlaufen - zurücksetzen
{
FlugMinuten
= 0;
FlugMinutenGesamt
= 0;
}
printf("\n\rFlight-time %u min Total:%u min" ,FlugMinuten
,FlugMinutenGesamt
);
if(eeprom_read_byte
(&EEPromArray
[EEPROM_ADR_ACC_NICK
]) > 4)
{
printf("\n\rACC noch nicht calibriert !");
}
ReadParameterSet
(GetActiveParamSetNumber
(), (unsigned char *) &EE_Parameter.
Kanalbelegung[0], STRUCT_PARAM_LAENGE
); // #define STRUCT_PARAM_LAENGE sizeof(EE_Parameter)
printf("\n\rUsing parameterset %d", GetActiveParamSetNumber
());
if(EE_Parameter.
GlobalConfig & CFG_HOEHENREGELUNG
)
{
printf("\n\rKalibrieren des Drucksensors.");
timer
= SetDelay
(1000); //
SucheLuftruckOffset
();
while (!CheckDelay
(timer
)); // pause 1 sec
printf("OK\n\r");
}
SetNeutral
(); // Nullwerte ermitteln und Startwerte festlegen - goto fc.c line 162
ROT_OFF
; // redLED off // PORTB |= 0x01;
beeptime
= 2000;
ExternControl.
Digital[0] = 0x55; // externe Steuerung per serieller Schnittstelle - siehe uart.h
printf("\n\rControl: ");
if (EE_Parameter.
GlobalConfig & CFG_HEADING_HOLD
) printf("HeadingHold");
else printf("normaler (ACC-Mode)");
LcdClear
();
I2CTimeout
= 5000; // watchdog set up to 10 sec
WinkelOut.
Orientation = 1;
LipoDetection
(1); // Lipos should be detected now
printf("\n\r===================================\n\r");
timer
= SetDelay
(2000);
// -------------------------------- end of main() prelude ---------------------------------------------------------------------
//----------------------------------------------------------------------------------------------------------------------
// forever loop of main program
//----------------------------------------------------------------------------------------------------------------------
while(1)
{
if(UpdateMotor
&& AdReady
) // motor is updated every 2 ms and ADC is already passed
{
UpdateMotor
=0; // reset and wait fpr the next 2ms timed trigger from timer0 IR
if(WinkelOut.
CalcState) CalMk3Mag
(); // In diesem Spezial-Fall soll der Kompass kalibriert werden
else MotorRegler
(); // Im Normalfall Sollwerte für die Motoren berechnen = goto fc.c line 541
SendMotorData
(); // Sollwerte an die Motorren senden -> fc.c Zeile 465
ROT_OFF
; // switch red LED off // PORTB |= 0x01;
if(SenderOkay
) SenderOkay
--; // ICIE1: Timer/Counter1, Input Capture Interrupt Enable
else TIMSK1
|= _BV
(ICIE1
); // enable PPM-Input // TIMSK1 Timer/Counter1 Interrupt Mask Register -> ICIE1 OCIE1B OCIE1A TOIE1
if(!--I2CTimeout
|| MissingMotor
) // counting down I2CTimeout or motor is missing
{
if(!I2CTimeout
)
{
i2c_reset
();
I2CTimeout
= 5;
DebugOut.
Analog[28]++; // I2C-Error
}
if((BeepMuster
== 0xffff) && MotorenEin
)
{
beeptime
= 10000;
BeepMuster
= 0x0080;
}
}
else
{
ROT_OFF
; // switch red LED off // PORTB |= 0x01;
}
if( 1 && (!UpdateMotor
|| !MotorenEin
))
{
DatenUebertragung
();
BearbeiteRxDaten
();
}
else BearbeiteRxDaten
();
// DatenUebertragung(); // where ist that contained?
// BearbeiteRxDaten(); // where ist that contained?
if(CheckDelay
(timer
)) // goto timer0.c line 65
{
timer
+= 20;
if(PcZugriff
) PcZugriff
--; // flight-CTRL controlled by external PC
else
{
ExternControl.
Config = 0;
ExternStickNick
= 0;
ExternStickRoll
= 0;
ExternStickGier
= 0;
if(BeepMuster
== 0xffff && SenderOkay
== 0)
{
beeptime
= 15000;
BeepMuster
= 0x0c00;
}
}
if(UBat
< BattLowVoltageWarning
) // low battery
{
MikroKopterFlags
|= FLAG_LOWBAT
;
if(BeepMuster
== 0xffff)
{
beeptime
= 6000;
BeepMuster
= 0x0300;
}
}
else MikroKopterFlags
&= ~FLAG_LOWBAT
;
//SPI_StartTransmitPacket(); // where ist that contained?
SendSPI
= 4;
if(!MotorenEin
) timer2
= 1450; // round it up to 30 sec
if(++timer2
== 2930) // one minute
{
timer2
= 0;
FlugMinuten
++;
FlugMinutenGesamt
++;
eeprom_write_byte
(&EEPromArray
[EEPROM_ADR_MINUTES2
],FlugMinuten
/ 256);
eeprom_write_byte
(&EEPromArray
[EEPROM_ADR_MINUTES2
+1],FlugMinuten
% 256);
eeprom_write_byte
(&EEPromArray
[EEPROM_ADR_MINUTES
],FlugMinutenGesamt
/ 256);
eeprom_write_byte
(&EEPromArray
[EEPROM_ADR_MINUTES
+1],FlugMinutenGesamt
% 256);
timer
= SetDelay
(20); // delay 20 ms
}
}
LED_Update
(); // junmps erery 2ms to led.c line 32
} // *** EOF : if(UpdateMotor && AdReady)
// *** EOF: if(!SendSPI) { SPI_TransmitByte(); }
} // End of endlessloop
return (1);
}
// *** EOF: main(void) ************************************************************************************************************