| 8,19 → 8,14 |
|---|
| #include "uart0.h" |
| #include "output.h" |
| #include "attitude.h" |
| #include "commands.h" |
| #include "flight.h" |
| #include "controlMixer.h" |
| #include "rc.h" |
| #include "analog.h" |
| #include "configuration.h" |
| #include "twimaster.h" |
| #ifdef USE_NAVICTRL |
| #include "spi.h" |
| #endif |
| #ifdef USE_MK3MAG |
| #include "mk3mag.h" |
| #endif |
| #include "controlMixer.h" |
| #include "eeprom.h" |
| #include "printf_P.h" |
| |
| int16_t main(void) { |
| uint16_t timer; |
| 29,8 → 24,8 |
|---|
| cli(); |
| |
| // analyze hardware environment |
| CPUType = getCPUType(); |
| BoardRelease = getBoardRelease(); |
| setCPUType(); |
| setBoardRelease(); |
| |
| // disable watchdog |
| MCUSR &= ~(1 << WDRF); |
| 37,31 → 32,35 |
|---|
| WDTCSR |= (1 << WDCE) | (1 << WDE); |
| WDTCSR = 0; |
| |
| // PPM_in[CH_THROTTLE] = 0; |
| // Why??? They are already initialized to 0. |
| // stickPitch = stickRoll = stickYaw = 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(); |
| |
| RED_OFF; |
| |
| // initalize modules |
| output_init(); |
| timer0_init(); |
| usart0_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 |
| |
| // Parameter Set handling |
| IMUConfig_readOrDefault(); |
| channelMap_readOrDefault(); |
| paramSet_readOrDefault(); |
| |
| // enable interrupts global |
| sei(); |
| |
| // Parameter Set handling |
| ParamSet_Init(); |
| 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); |
| 69,75 → 68,86 |
|---|
| |
| // Instead, while away the time by flashing the 2 outputs: |
| // First J16, then J17. Makes it easier to see which is which. |
| timer = setDelay(500); |
| OUTPUT_SET(0,1); |
| timer = setDelay(200); |
| outputSet(0,1); |
| GRN_OFF; |
| RED_ON; |
| while (!checkDelay(timer)) |
| ; |
| |
| OUTPUT_SET(0,0); |
| |
| timer = setDelay(500); |
| while (!checkDelay(timer)) |
| ; |
| |
| OUTPUT_SET(1,1); |
| timer = setDelay(200); |
| outputSet(0,0); |
| outputSet(1,1); |
| RED_OFF; |
| GRN_ON; |
| timer = setDelay(500); |
| while (!checkDelay(timer)) |
| ; |
| |
| timer = setDelay(500); |
| timer = setDelay(200); |
| while (!checkDelay(timer)) |
| ; |
| outputSet(1,0); |
| GRN_OFF; |
| |
| OUTPUT_SET(1,0); |
| printf("\n\r==================================="); |
| |
| beep(2000); |
| #ifdef USE_NAVICTRL |
| printf("\n\rSupport for NaviCtrl"); |
| #endif |
| |
| timer = setDelay(4000); |
| while (!checkDelay(timer)) |
| ; |
| #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(); |
| // flight_setNeutral(); |
| |
| // RED_OFF; |
| beep(2000); |
| |
| beep(1000); |
| timer2_init(); |
| printf("\n\rControl: "); |
| if (staticParams.bitConfig & CFG_HEADING_HOLD) |
| printf("Heading Hold"); |
| else printf("RTL Mode"); |
| |
| I2CTimeout = 5000; |
| printf("\n\n\r"); |
| |
| while (1) { |
| while (1) { |
| if (runFlightControl) { // control interval |
| runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0 |
| if (!analogDataReady) { |
| DebugOut.Digital[0] |= DEBUG_MAINLOOP_TIMER; |
| // Analog data should have been ready but is not!! |
| debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER; |
| } else { |
| DebugOut.Digital[0] &= ~DEBUG_MAINLOOP_TIMER; |
| 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; |
| |
| |
| // Allow Serial Data Transmit if motors must not updated or motors are not running |
| if (!runFlightControl || !(MKFlags & MKFLAG_MOTOR_RUN)) { |
| usart0_TransmitTxData(); |
| usart0_transmitTxData(); |
| } |
| |
| usart0_ProcessRxData(); |
| 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.LowVoltageWarning) { |
| } else if (UBat < staticParams.batteryVoltageWarning) { |
| beepBatteryAlarm(); |
| } |
| |
| 159,6 → 169,12 |
|---|
| SPI_TransmitByte(); |
| } |
| #endif |
| |
| 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); |