19,7 → 19,6 |
|
int16_t main(void) { |
uint16_t timer; |
static uint8_t profileTimer; |
|
// disable interrupts global |
cli(); |
31,7 → 30,7 |
|
// 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(); |
channelMap_default(); |
|
// initalize modules |
output_init(); |
44,20 → 43,68 |
// 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: Atmega328"); |
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(); |
|
// This is not a busy-wait operation and should be OK. |
// 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 |
85,21 → 132,18 |
|
usart0_processRxData(); |
|
static uint8_t aboveWarningLimitVoltageSeen = 0; |
if (checkDelay(timer)) { |
// 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.batteryWarningVoltage && UBat > UBAT_AT_5V) { |
beepBatteryAlarm(); |
} |
|
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 |
} |
timer = setDelay(20); // every 20 ms |
output_update(); |
} |
|
output_update(); |
calculateFeaturedServoValues(); |
|
if (runFlightControl) { // Time for the next iteration was up before the current finished. Signal error. |
debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER; |