Rev 2124 |
Rev 2132 |
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 "output.h"
#include "attitude.h"
#include "commands.h"
#include "flight.h"
#include "rc.h"
#include "analog.h"
#include "configuration.h"
#include "controlMixer.h"
#include "eeprom.h"
#include "printf_P.h"
int16_t main
(void) {
uint16_t timer
;
static uint8_t profileTimer
;
// 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
();
// Parameter Set handling
IMUConfig_readOrDefault
();
channelMap_readOrDefault
();
rcTrim_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
;
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\n\r");
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
;
}
// 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.
#ifdef DO_PROFILE
startProfileTimer
(ATTITUDE
);
#endif
calculateFlightAttitude
();
#ifdef DO_PROFILE
stopProfileTimer
(ATTITUDE
);
#endif
#ifdef DO_PROFILE
startProfileTimer
(CONTROLMIXER
);
#endif
controlMixer_periodicTask
();
#ifdef DO_PROFILE
stopProfileTimer
(CONTROLMIXER
);
#endif
#ifdef DO_PROFILE
startProfileTimer
(COMMANDS
);
#endif
commands_handleCommands
();
#ifdef DO_PROFILE
stopProfileTimer
(COMMANDS
);
#endif
#ifdef DO_PROFILE
startProfileTimer
(FLIGHT
);
#endif
flight_control
();
#ifdef DO_PROFILE
stopProfileTimer
(FLIGHT
);
#endif
// Allow Serial Data Transmit if motors must not updated or motors are not running
if (!runFlightControl
|| !isFlying
) {
usart0_transmitTxData
();
}
usart0_processRxData
();
static uint8_t aboveWarningLimitVoltageSeen
= 0;
if (checkDelay
(timer
)) {
if (UBat
>= staticParams.
batteryWarningVoltage) {
aboveWarningLimitVoltageSeen
= 1;
} else { // If we are above USB voltage, or if we have once been above warning voltage
if (aboveWarningLimitVoltageSeen
|| UBat
> UBAT_AT_5V
) {
beepBatteryAlarm
();
}
}
calculateFeaturedServoValues
();
timer
= setDelay
(20); // every 20 ms
}
output_update
();
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);
}