WebSVN - FlightCtrl - Diff - Rev 2097 and 2189 - /branches/dongfang_FC

 #include <inttypes.h>
 #include <avr/io.h>
 #include <avr/interrupt.h>
-#include "eeprom.h"
+//#include "eeprom.h"
-#include "analog.h"
+#include "profiler.h"
 #include "controlMixer.h"
+#include "configuration.h"
+#include "analog.h"
 #include "timer0.h"
+#include "debug.h"
+#include "beeper.h"
 #include "output.h"
+#include "commands.h"
+#include "flight.h"
+#include "uart0.h"
+#include "twimaster.h"
 #ifdef USE_MK3MAG
 #include "mk3mag.h"
+#endif
+#define MILLIS_DIVIDER 10
-#endif
+volatile uint32_t jiffiesClock;
-volatile uint32_t globalMillisClock = 0;
+volatile uint32_t millisClock;
-volatile uint8_t  runFlightControl = 0;
+volatile uint8_t  loopJiffiesClock;
-volatile uint16_t beepTime = 0;
-volatile uint16_t beepModulation = BEEP_MODULATION_NONE;
+volatile uint16_t beepTime;
-#ifdef USE_NAVICTRL
+volatile uint16_t beepModulation = BEEP_MODULATION_NONE;
 volatile uint8_t SendSPI = 0;
-#endif
+volatile uint8_t flightControlStatus;
-  // Timer/Counter 0 Control Register B
+        // Timer/Counter 0 Control Register B
-  // set clock divider for timer 0 to SYSCLOCK/8 = 20MHz/8 = 2.5MHz
+        // set clock divider for timer 0 to SYSCLOCK/8 = 20MHz/8 = 2.5MHz
-  // i.e. the timer increments from 0x00 to 0xFF with an update rate of 2.5 MHz
+        // i.e. the timer increments from 0x00 to 0xFF with an update rate of 2.5 MHz
-  // hence the timer overflow interrupt frequency is 2.5 MHz/256 = 9.765 kHz
         // hence the timer overflow interrupt frequency is 2.5 MHz/256 = 9.765 kHz
-  // divider 8 (Bits CS02 = 0, CS01 = 1, CS00 = 0)
+        // divider 8 (Bits CS02 = 0, CS01 = 1, CS00 = 0)
-  TCCR0B &= ~((1 << FOC0A) | (1 << FOC0B) | (1 << WGM02));
+        TCCR0B &= ~((1 << FOC0A) | (1 << FOC0B) | (1 << WGM02));
-  TCCR0B = (TCCR0B & 0xF8) | (0 << CS02) | (1 << CS01) | (0 << CS00);
+        TCCR0B = (TCCR0B & 0xF8) | (0 << CS02) | (1 << CS01) | (0 << CS00);
-  TIMSK0 |= (1 << TOIE0);
+        TIMSK0 |= (1 << TOIE0);
-  SREG = sreg;
+        SREG = sreg;
+}
+void runFlightControlTask(void) {
+        if (flightControlStatus != NOT_RUNNING) {
+                // Previous execution not completed! It is dangerous to start another.
+                debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER;
+                return;
+        }
+        debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER;
+        controlMixer_periodicTask();
+        commands_handleCommands();
+        flightControlStatus = RUNNING;
+    if (!--I2CTimeout || missingMotor) { // try to reset the i2c if motor is missing or timeout
+         if (!I2CTimeout) {
+             I2C_reset();
+             I2CTimeout = 5;
+         }
+         beepI2CAlarm();
+     }
+        if (analog_controlDataStatus != CONTROL_SENSOR_DATA_READY) {
+                // Analog data should have been ready but is not!!
+                debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER;
+        } else {
+                debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER;
+                J4HIGH;
+                analog_sumAttitudeData();
+                analog_updateControlData();
+                flight_control();
+                output_applyMulticopterMixer();
+                I2C_start(TWI_STATE_MOTOR_TX);
+                J4LOW;
+        }
+        flightControlStatus = NOT_RUNNING;
+}
 /*****************************************************/
 /*          Interrupt Routine of Timer 0             */
 /*****************************************************/
+ISR (TIMER0_OVF_vect) { // 9765.625 Hz
+        static uint8_t millisDivider = MILLIS_DIVIDER;
+        static uint8_t controlLoopDivider = CONTROLLOOP_DIVIDER;
-ISR(TIMER0_OVF_vect) { // 9765.625 Hz
+        static uint8_t serialLoopDivider = SERIALLOOP_DIVIDER /2;
-  static uint8_t cnt_1ms = 1, cnt = 0;
+    static uint8_t outputLoopDivider = OUTPUTLOOP_DIVIDER /3;
+        uint8_t beeperOn = 0;
-  uint8_t beeperOn = 0;
+        jiffiesClock++;
+        loopJiffiesClock++;
-#ifdef USE_NAVICTRL
+        // profiler_scoreTimerHit();
-  if(SendSPI) SendSPI--; // if SendSPI is 0, the transmit of a byte via SPI bus to and from The Navicontrol is done
-#endif
+        sei();
+        if (!--millisDivider) {
+                millisClock++;
+                millisDivider = MILLIS_DIVIDER;
-  if (!cnt--) { // every 10th run (9.765625kHz/10 = 976.5625Hz)
+        }
-    cnt = 9;
+        if (!--controlLoopDivider) {
+            //sei();
+                controlLoopDivider= CONTROLLOOP_DIVIDER;
+                runFlightControlTask();
-    cnt_1ms ^= 1;
+                //cli();
-    if (!cnt_1ms) {
+        }
-      if (runFlightControl == 1)
-        debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER;
+    if (!--serialLoopDivider) {
+        //sei();
+        serialLoopDivider= SERIALLOOP_DIVIDER;
+        // Allow serial data transmission if there is still time, or if we are not flying anyway.
-      else
+        usart0_transmitTxData();
+        usart0_processRxData();
+        //cli();
+    }
-        debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER;
+    if (!--outputLoopDivider) {
+        //sei();
-      runFlightControl = 1; // every 2nd run (976.5625 Hz/2 = 488.28125 Hz)
+        outputLoopDivider= OUTPUTLOOP_DIVIDER;
         output_update();
-    globalMillisClock++; // increment millisecond counter
+        //cli();
+    }
 #endif
+}
 // -----------------------------------------------------------------------
 uint16_t setDelay(uint16_t t) {
-  return (globalMillisClock + t - 1);
+        return (millisClock + t - 1);
+}
 // -----------------------------------------------------------------------
 int8_t checkDelay(uint16_t t) {
-  return (((t - globalMillisClock) & 0x8000) >> 8); // check sign bit
+        return (((t - millisClock) & 0x8000) >> 8); // check sign bit
+}
 // -----------------------------------------------------------------------
 void delay_ms(uint16_t w) {
-  uint16_t t_stop = setDelay(w);
+        uint16_t t_stop = setDelay(w);
-  while (!checkDelay(t_stop))
-    ;
-}
-// -----------------------------------------------------------------------
-void delay_ms_with_adc_measurement(uint16_t w, uint8_t stop) {
-  uint16_t t_stop;
-  t_stop = setDelay(w);
-  while (!checkDelay(t_stop)) {
-        if (analogDataReady) {
-          analog_update();
-          startAnalogConversionCycle();
-        }
-  }
-  if (stop) {
-  // Wait for new samples to get prepared but do not restart AD conversion after that!
-  // Caller MUST to that.

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/timer0.c - Rev 2097 → 2189