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); |
} |