Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2131 → Rev 2132

/branches/dongfang_FC_fixedwing/main.c
1,6 → 1,7
#include <avr/boot.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/wdt.h>
#include <util/delay.h>
 
#include "timer0.h"
17,130 → 18,139
#include "eeprom.h"
#include "printf_P.h"
 
uint8_t resetFlag = 0;
 
void reset(void) {
resetFlag = 1;
wdt_enable(WDTO_15MS);
while (1)
;
}
 
int16_t main(void) {
uint16_t timer;
static uint8_t profileTimer;
uint16_t timer = 0;
 
// disable interrupts global
cli();
#ifdef DO_PROFILE
static uint8_t profileTimer;
#endif
 
// analyze hardware environment
setCPUType();
setBoardRelease();
// disable interrupts global
cli();
 
// disable watchdog
MCUSR &= ~(1 << WDRF);
WDTCSR |= (1 << WDCE) | (1 << WDE);
WDTCSR = 0;
wdt_enable(WDTO_120MS);
 
// 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();
// analyze hardware environment
setCPUType();
setBoardRelease();
 
// initalize modules
output_init();
timer0_init();
timer2_init();
usart0_init();
//if (CPUType == ATMEGA644P);// usart1_Init();
RC_Init();
analog_init();
// 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();
// Parameter Set handling
IMUConfig_readOrDefault();
channelMap_readOrDefault();
rcTrim_readOrDefault();
paramSet_readOrDefault();
 
// enable interrupts global
sei();
// enable interrupts global
sei();
 
controlMixer_setNeutral();
controlMixer_setNeutral();
 
// Cal. attitude sensors and reset integrals.
attitude_setNeutral();
// Cal. attitude sensors and reset integrals.
attitude_setNeutral();
 
// Init flight parameters
// flight_setNeutral();
// Init flight parameters
// flight_setNeutral();
 
// This is not a busy-wait operation and should be OK.
beep(2000);
// This is not a busy-wait operation and should be OK.
beep(2000);
 
while (1) {
if (runFlightControl) { // control interval
runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
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.
if (!resetFlag) {
wdt_reset();
}
 
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);
startProfileTimer(ATTITUDE);
#endif
calculateFlightAttitude();
calculateFlightAttitude();
#ifdef DO_PROFILE
stopProfileTimer(ATTITUDE);
stopProfileTimer(ATTITUDE);
#endif
 
#ifdef DO_PROFILE
startProfileTimer(CONTROLMIXER);
startProfileTimer(CONTROLMIXER);
#endif
controlMixer_periodicTask();
controlMixer_periodicTask();
#ifdef DO_PROFILE
stopProfileTimer(CONTROLMIXER);
stopProfileTimer(CONTROLMIXER);
#endif
 
#ifdef DO_PROFILE
startProfileTimer(COMMANDS);
startProfileTimer(COMMANDS);
#endif
commands_handleCommands();
commands_handleCommands();
#ifdef DO_PROFILE
stopProfileTimer(COMMANDS);
stopProfileTimer(COMMANDS);
#endif
 
#ifdef DO_PROFILE
startProfileTimer(FLIGHT);
startProfileTimer(FLIGHT);
#endif
flight_control();
flight_control();
#ifdef DO_PROFILE
stopProfileTimer(FLIGHT);
stopProfileTimer(FLIGHT);
#endif
 
// Allow Serial Data Transmit if motors must not updated or motors are not running
if (!runFlightControl || !isFlying) {
usart0_transmitTxData();
}
// Allow Serial Data Transmit if motors must not updated or motors are not running
if (!runFlightControl || !isFlying) {
usart0_transmitTxData();
}
 
usart0_processRxData();
usart0_processRxData();
 
static uint8_t aboveWarningLimitVoltageSeen = 0;
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
}
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();
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);
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);
}