Rev 2055 |
Rev 2092 |
Go to most recent revision |
Blame |
Compare with Previous |
Last modification |
View Log
| RSS feed
#include <avr/boot.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include "timer0.h"
#include "timer2.h"
#include "uart0.h"
#include "uart1.h"
#include "output.h"
#include "menu.h"
#include "attitude.h"
#include "commands.h"
#include "flight.h"
#include "rc.h"
#include "analog.h"
#include "configuration.h"
#include "printf_P.h"
#include "twimaster.h"
#include "controlMixer.h"
#ifdef USE_NAVICTRL
#include "spi.h"
#endif
#ifdef USE_MK3MAG
#include "mk3mag.h"
#endif
#include "eeprom.h"
int16_t main
(void) {
uint16_t timer
;
// disable interrupts global
cli
();
// analyze hardware environment
setCPUType
();
setBoardRelease
();
// disable watchdog
MCUSR
&= ~
(1 << WDRF
);
WDTCSR
|= (1 << WDCE
) | (1 << WDE
);
WDTCSR
= 0;
// This is strange: It should NOT be necessarty to do. But the call of the same,
// in channelMap_readOrDefault (if eeprom read fails) just sets all to 0,0,0,....
channelMap_default
();
// initalize modules
output_init
();
timer0_init
();
timer2_init
();
usart0_init
();
//if (CPUType == ATMEGA644P);// usart1_Init();
RC_Init
();
analog_init
();
I2C_init
();
#ifdef USE_NAVICTRL
SPI_MasterInit
();
#endif
#ifdef USE_MK3MAG
MK3MAG_init
();
#endif
#ifdef USE_DIRECT_GPS
usart1_init
();
#endif
// Parameter Set handling
channelMap_readOrDefault
();
mixerMatrix_readOrDefault
();
paramSet_readOrDefault
();
// enable interrupts global
sei
();
printf("\n\r===================================");
printf("\n\rFlightControl");
printf("\n\rHardware: Custom");
printf("\n\r CPU: Atmega644");
if (CPUType
== ATMEGA644P
)
printf("p");
printf("\n\rSoftware: V%d.%d%c",VERSION_MAJOR
, VERSION_MINOR
, VERSION_PATCH
+ 'a');
printf("\n\r===================================");
// Wait for a short time (otherwise the RC channel check won't work below)
// timer = SetDelay(500);
// while(!CheckDelay(timer));
// Instead, while away the time by flashing the 2 outputs:
// First J16, then J17. Makes it easier to see which is which.
timer
= setDelay
(200);
outputSet
(0,1);
GRN_OFF
;
RED_ON
;
while (!checkDelay
(timer
))
;
timer
= setDelay
(200);
outputSet
(0,0);
outputSet
(1,1);
RED_OFF
;
GRN_ON
;
while (!checkDelay
(timer
))
;
timer
= setDelay
(200);
while (!checkDelay
(timer
))
;
outputSet
(1,0);
GRN_OFF
;
twi_diagnostics
();
printf("\n\r===================================");
#ifdef USE_NAVICTRL
printf("\n\rSupport for NaviCtrl");
#endif
#ifdef USE_DIRECT_GPS
printf("\n\rDirect (no NaviCtrl) navigation");
#endif
controlMixer_setNeutral
();
// Cal. attitude sensors and reset integrals.
attitude_setNeutral
();
// Init flight parameters
// flight_setNeutral();
beep
(2000);
printf("\n\rControl: ");
if (staticParams.
bitConfig & CFG_HEADING_HOLD
)
printf("Heading Hold");
else printf("RTL Mode");
printf("\n\n\r");
LCD_clear
();
I2CTimeout
= 5000;
while (1) {
if (runFlightControl
) { // control interval
runFlightControl
= 0; // reset Flag, is enabled every 2 ms by ISR of timer0
if (!analogDataReady
) {
// Analog data should have been ready but is not!!
debugOut.
digital[0] |= DEBUG_MAINLOOP_TIMER
;
} else {
debugOut.
digital[0] &= ~DEBUG_MAINLOOP_TIMER
;
J4HIGH
;
// This is probably the correct order:
// The attitude computation should not depend on anything from control (except maybe the estimation of control activity level)
// The control may depend on attitude - for example, attitude control uses pitch and roll angles, compass control uses yaw angle etc.
// Flight control uses results from both.
calculateFlightAttitude
();
controlMixer_periodicTask
();
commands_handleCommands
();
flight_control
();
J4LOW
;
if (!--I2CTimeout
|| missingMotor
) { // try to reset the i2c if motor is missing or timeout
if (!I2CTimeout
) {
I2C_Reset
();
I2CTimeout
= 5;
}
beepI2CAlarm
();
}
// Allow Serial Data Transmit if motors must not updated or motors are not running
if (!runFlightControl
|| !(MKFlags
& MKFLAG_MOTOR_RUN
)) {
usart0_transmitTxData
();
}
usart0_processRxData
();
if (checkDelay
(timer
)) {
if (UBat
<= UBAT_AT_5V
) {
// Do nothing. The voltage on the input side of the regulator is <5V;
// we must be running off USB power. Keep it quiet.
} else if (UBat
< staticParams.
batteryVoltageWarning) {
beepBatteryAlarm
();
}
#ifdef USE_NAVICTRL
SPI_StartTransmitPacket
();
SendSPI
= 4;
#endif
timer
= setDelay
(20); // every 20 ms
}
output_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
calculateServoValues
();
if (runFlightControl
) { // Time for the next iteration was up before the current finished. Signal error.
debugOut.
digital[1] |= DEBUG_MAINLOOP_TIMER
;
} else {
debugOut.
digital[1] &= ~DEBUG_MAINLOOP_TIMER
;
}
}
}
return (1);
}