Subversion Repositories FlightCtrl

Rev

Rev 2097 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 2097 Rev 2189
Line 1... Line 1...
1
#include <inttypes.h>
1
#include <inttypes.h>
2
#include <avr/io.h>
2
#include <avr/io.h>
3
#include <avr/interrupt.h>
3
#include <avr/interrupt.h>
4
#include "eeprom.h"
4
//#include "eeprom.h"
5
#include "analog.h"
5
#include "profiler.h"
6
#include "controlMixer.h"
6
#include "controlMixer.h"
7
 
-
 
-
 
7
#include "configuration.h"
-
 
8
#include "analog.h"
8
#include "timer0.h"
9
#include "timer0.h"
-
 
10
#include "debug.h"
-
 
11
#include "beeper.h"
9
#include "output.h"
12
#include "output.h"
-
 
13
#include "commands.h"
-
 
14
#include "flight.h"
-
 
15
#include "uart0.h"
-
 
16
#include "twimaster.h"
Line 10... Line 17...
10
 
17
 
11
#ifdef USE_MK3MAG
18
#ifdef USE_MK3MAG
12
#include "mk3mag.h"
19
#include "mk3mag.h"
Line -... Line 20...
-
 
20
#endif
-
 
21
 
-
 
22
#define MILLIS_DIVIDER 10
13
#endif
23
 
14
 
24
volatile uint32_t jiffiesClock;
15
volatile uint32_t globalMillisClock = 0;
25
volatile uint32_t millisClock;
16
volatile uint8_t  runFlightControl = 0;
26
volatile uint8_t  loopJiffiesClock;
Line 17... Line -...
17
volatile uint16_t beepTime = 0;
-
 
18
volatile uint16_t beepModulation = BEEP_MODULATION_NONE;
27
volatile uint16_t beepTime;
19
 
-
 
Line 20... Line 28...
20
#ifdef USE_NAVICTRL
28
volatile uint16_t beepModulation = BEEP_MODULATION_NONE;
21
volatile uint8_t SendSPI = 0;
29
 
22
#endif
30
volatile uint8_t flightControlStatus;
23
 
31
 
24
/*****************************************************
32
/*****************************************************
25
 * Initialize Timer 0                  
33
 * Initialize Timer 0                  
26
 *****************************************************/
34
 *****************************************************/
Line 27... Line 35...
27
// timer 0 is used for the PWM generation to control the offset voltage at the air pressure sensor
35
// timer 0 is used for the PWM generation to control the offset voltage at the air pressure sensor
28
// Its overflow interrupt routine is used to generate the beep signal and the flight control motor update rate
36
// Its overflow interrupt routine is used to generate the beep signal and the flight control motor update rate
Line 29... Line 37...
29
void timer0_init(void) {
37
void timer0_init(void) {
30
  uint8_t sreg = SREG;
38
        uint8_t sreg = SREG;
31
 
39
 
32
  // disable all interrupts before reconfiguration
40
        // disable all interrupts before reconfiguration
33
  cli();
41
        cli();
34
 
42
 
35
  // Configure speaker port as output.
43
        // Configure speaker port as output.
36
  if (boardRelease == 10) { // Speaker at PD2
44
        if (boardRelease == 10) { // Speaker at PD2
37
    DDRD |= (1 << DDD2);
45
                DDRD |= (1 << DDD2);
38
    PORTD &= ~(1 << PORTD2);
46
                PORTD &= ~(1 << PORTD2);
39
  } else { // Speaker at PC7
47
        } else { // Speaker at PC7
40
    DDRC |= (1 << DDC7);
48
                DDRC |= (1 << DDC7);
41
    PORTC &= ~(1 << PORTC7);
49
                PORTC &= ~(1 << PORTC7);
42
  }
50
        }
43
 
51
 
44
  // set PB3 and PB4 as output for the PWM used as offset for the pressure sensor
52
        // set PB3 and PB4 as output for the PWM used as offset for the pressure sensor
45
  DDRB |= (1 << DDB4) | (1 << DDB3);
53
        DDRB |= (1 << DDB4) | (1 << DDB3);
46
  PORTB &= ~((1 << PORTB4) | (1 << PORTB3));
54
        PORTB &= ~((1 << PORTB4) | (1 << PORTB3));
47
 
55
 
48
  // Timer/Counter 0 Control Register A
56
        // Timer/Counter 0 Control Register A
49
 
57
 
50
  // Waveform Generation Mode is Fast PWM (Bits WGM02 = 0, WGM01 = 1, WGM00 = 1)
58
        // Waveform Generation Mode is Fast PWM (Bits WGM02 = 0, WGM01 = 1, WGM00 = 1)
51
  // Clear OC0A on Compare Match, set OC0A at BOTTOM, noninverting PWM (Bits COM0A1 = 1, COM0A0 = 0)
59
        // Clear OC0A on Compare Match, set OC0A at BOTTOM, noninverting PWM (Bits COM0A1 = 1, COM0A0 = 0)
52
  // Clear OC0B on Compare Match, set OC0B at BOTTOM, (Bits COM0B1 = 1, COM0B0 = 0)
60
        // Clear OC0B on Compare Match, set OC0B at BOTTOM, (Bits COM0B1 = 1, COM0B0 = 0)
53
  TCCR0A &= ~((1 << COM0A0) | (1 << COM0B0));
61
        TCCR0A &= ~((1 << COM0A0) | (1 << COM0B0));
54
  TCCR0A |= (1 << COM0A1) | (1 << COM0B1) | (1 << WGM01) | (1 << WGM00);
-
 
55
 
62
        TCCR0A |= (1 << COM0A1) | (1 << COM0B1) | (1 << WGM01) | (1 << WGM00);
56
  // Timer/Counter 0 Control Register B
63
 
57
  // set clock divider for timer 0 to SYSCLOCK/8 = 20MHz/8 = 2.5MHz
64
        // Timer/Counter 0 Control Register B
58
  // i.e. the timer increments from 0x00 to 0xFF with an update rate of 2.5 MHz
65
        // set clock divider for timer 0 to SYSCLOCK/8 = 20MHz/8 = 2.5MHz
59
  // hence the timer overflow interrupt frequency is 2.5 MHz/256 = 9.765 kHz
66
        // i.e. the timer increments from 0x00 to 0xFF with an update rate of 2.5 MHz
60
 
67
        // hence the timer overflow interrupt frequency is 2.5 MHz/256 = 9.765 kHz
61
  // divider 8 (Bits CS02 = 0, CS01 = 1, CS00 = 0)
68
        // divider 8 (Bits CS02 = 0, CS01 = 1, CS00 = 0)
62
  TCCR0B &= ~((1 << FOC0A) | (1 << FOC0B) | (1 << WGM02));
69
        TCCR0B &= ~((1 << FOC0A) | (1 << FOC0B) | (1 << WGM02));
63
  TCCR0B = (TCCR0B & 0xF8) | (0 << CS02) | (1 << CS01) | (0 << CS00);
70
        TCCR0B = (TCCR0B & 0xF8) | (0 << CS02) | (1 << CS01) | (0 << CS00);
64
 
71
 
65
  // initialize the Output Compare Register A & B used for PWM generation on port PB3 & PB4
72
        // initialize the Output Compare Register A & B used for PWM generation on port PB3 & PB4
66
  OCR0A = 0; // for PB3
73
        OCR0A = 0; // for PB3
67
  OCR0B = 120; // for PB4
74
        OCR0B = 120; // for PB4
68
 
75
 
69
  // init Timer/Counter 0 Register
76
        // init Timer/Counter 0 Register
-
 
77
        TCNT0 = 0;
-
 
78
 
-
 
79
        // Timer/Counter 0 Interrupt Mask Register
-
 
80
        // enable timer overflow interrupt only
-
 
81
        TIMSK0 &= ~((1 << OCIE0B) | (1 << OCIE0A));
-
 
82
        TIMSK0 |= (1 << TOIE0);
-
 
83
 
-
 
84
        SREG = sreg;
-
 
85
}
-
 
86
 
-
 
87
void runFlightControlTask(void) {
-
 
88
        if (flightControlStatus != NOT_RUNNING) {
-
 
89
                // Previous execution not completed! It is dangerous to start another.
-
 
90
                debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER;
-
 
91
                return;
-
 
92
        }
-
 
93
 
-
 
94
        debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER;
-
 
95
 
-
 
96
        controlMixer_periodicTask();
-
 
97
        commands_handleCommands();
-
 
98
        flightControlStatus = RUNNING;
-
 
99
 
-
 
100
    if (!--I2CTimeout || missingMotor) { // try to reset the i2c if motor is missing or timeout
-
 
101
         if (!I2CTimeout) {
-
 
102
             I2C_reset();
-
 
103
             I2CTimeout = 5;
-
 
104
         }
-
 
105
         beepI2CAlarm();
-
 
106
     }
-
 
107
       
-
 
108
        if (analog_controlDataStatus != CONTROL_SENSOR_DATA_READY) {
-
 
109
                // Analog data should have been ready but is not!!
-
 
110
                debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER;
-
 
111
        } else {
-
 
112
                debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER;
-
 
113
                J4HIGH;
-
 
114
                analog_sumAttitudeData();
Line 70... Line 115...
70
  TCNT0 = 0;
115
                analog_updateControlData();
71
 
116
                flight_control();
Line 72... Line 117...
72
  // Timer/Counter 0 Interrupt Mask Register
117
                output_applyMulticopterMixer();
73
  // enable timer overflow interrupt only
118
                I2C_start(TWI_STATE_MOTOR_TX);
74
  TIMSK0 &= ~((1 << OCIE0B) | (1 << OCIE0A));
119
                J4LOW;
75
  TIMSK0 |= (1 << TOIE0);
120
        }
76
 
121
 
-
 
122
        flightControlStatus = NOT_RUNNING;
-
 
123
}
-
 
124
 
77
  SREG = sreg;
125
/*****************************************************/
78
}
126
/*          Interrupt Routine of Timer 0             */
79
 
127
/*****************************************************/
-
 
128
ISR (TIMER0_OVF_vect) { // 9765.625 Hz
-
 
129
        static uint8_t millisDivider = MILLIS_DIVIDER;
-
 
130
        static uint8_t controlLoopDivider = CONTROLLOOP_DIVIDER;
-
 
131
        static uint8_t serialLoopDivider = SERIALLOOP_DIVIDER /2;
-
 
132
    static uint8_t outputLoopDivider = OUTPUTLOOP_DIVIDER /3;
-
 
133
        uint8_t beeperOn = 0;
-
 
134
 
-
 
135
        jiffiesClock++;
-
 
136
        loopJiffiesClock++;
-
 
137
        // profiler_scoreTimerHit();
-
 
138
 
-
 
139
        sei();
-
 
140
 
-
 
141
        if (!--millisDivider) {
-
 
142
                millisClock++;
-
 
143
                millisDivider = MILLIS_DIVIDER;
-
 
144
        }
-
 
145
 
-
 
146
        if (!--controlLoopDivider) {
-
 
147
            //sei();
80
/*****************************************************/
148
                controlLoopDivider= CONTROLLOOP_DIVIDER;
-
 
149
                runFlightControlTask();
-
 
150
                //cli();
-
 
151
        }
81
/*          Interrupt Routine of Timer 0             */
152
 
Line 82... Line 153...
82
/*****************************************************/
153
    if (!--serialLoopDivider) {
83
ISR(TIMER0_OVF_vect) { // 9765.625 Hz
-
 
84
  static uint8_t cnt_1ms = 1, cnt = 0;
154
        //sei();
85
  uint8_t beeperOn = 0;
155
        serialLoopDivider= SERIALLOOP_DIVIDER;
86
 
156
        // Allow serial data transmission if there is still time, or if we are not flying anyway.
87
#ifdef USE_NAVICTRL
-
 
88
  if(SendSPI) SendSPI--; // if SendSPI is 0, the transmit of a byte via SPI bus to and from The Navicontrol is done
157
        usart0_transmitTxData();
89
#endif
-
 
90
 
-
 
91
  if (!cnt--) { // every 10th run (9.765625kHz/10 = 976.5625Hz)
158
        usart0_processRxData();
92
    cnt = 9;
-
 
93
    cnt_1ms ^= 1;
-
 
Line 94... Line 159...
94
    if (!cnt_1ms) {
159
        //cli();
95
      if (runFlightControl == 1)
160
    }
96
        debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER;
161
 
97
      else
162
    if (!--outputLoopDivider) {
98
        debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER;
163
        //sei();
99
      runFlightControl = 1; // every 2nd run (976.5625 Hz/2 = 488.28125 Hz)
164
        outputLoopDivider= OUTPUTLOOP_DIVIDER;
100
    }
165
        output_update();
101
    globalMillisClock++; // increment millisecond counter
166
        //cli();
102
  }
167
    }
103
 
168
 
104
  // beeper on if duration is not over
169
        // beeper on if duration is not over
105
  if (beepTime) {
170
        if (beepTime) {
106
    beepTime--; // decrement BeepTime
171
                beepTime--; // decrement BeepTime
107
    if (beepTime & beepModulation)
172
                if (beepTime & beepModulation)
108
      beeperOn = 1;
173
                        beeperOn = 1;
109
    else
174
                else
110
      beeperOn = 0;
175
                        beeperOn = 0;
111
  } else { // beeper off if duration is over
176
        } else { // beeper off if duration is over
112
    beeperOn = 0;
177
                beeperOn = 0;
113
    beepModulation = BEEP_MODULATION_NONE;
178
                beepModulation = BEEP_MODULATION_NONE;
114
  }
179
        }
115
 
180
 
116
  if (beeperOn) {
181
        if (beeperOn) {
117
    // set speaker port to high.
182
                // set speaker port to high.
118
    if (boardRelease == 10)
183
                if (boardRelease == 10)
Line 119... Line 184...
119
      PORTD |= (1 << PORTD2); // Speaker at PD2
184
                        PORTD |= (1 << PORTD2); // Speaker at PD2
120
    else
185
                else
121
      PORTC |= (1 << PORTC7); // Speaker at PC7
186
                        PORTC |= (1 << PORTC7); // Speaker at PC7
122
  } else { // beeper is off
187
        } else { // beeper is off
123
    // set speaker port to low
188
                // set speaker port to low
124
    if (boardRelease == 10)
189
                if (boardRelease == 10)
125
      PORTD &= ~(1 << PORTD2);// Speaker at PD2
190
                        PORTD &= ~(1 << PORTD2);// Speaker at PD2
Line 126... Line 191...
126
    else
191
                else
127
      PORTC &= ~(1 << PORTC7);// Speaker at PC7
192
                        PORTC &= ~(1 << PORTC7);// Speaker at PC7
128
  }
193
        }
129
 
194
 
Line 130... Line 195...
130
#ifdef USE_MK3MAG
195
#ifdef USE_MK3MAG
131
  // update compass value if this option is enabled in the settings
196
        // update compass value if this option is enabled in the settings
132
  if (staticParams.bitConfig & CFG_COMPASS_ENABLED) {
197
        if (staticParams.bitConfig & CFG_COMPASS_ENABLED) {
133
    MK3MAG_periodicTask(); // read out mk3mag pwm
198
                MK3MAG_periodicTask(); // read out mk3mag pwm
Line 134... Line 199...
134
  }
199
        }
135
#endif
200
#endif
136
}
201
}
137
 
202
 
138
// -----------------------------------------------------------------------
-
 
139
uint16_t setDelay(uint16_t t) {
-
 
140
  return (globalMillisClock + t - 1);
-
 
141
}
-
 
142
 
-
 
143
// -----------------------------------------------------------------------
-
 
144
int8_t checkDelay(uint16_t t) {
-
 
145
  return (((t - globalMillisClock) & 0x8000) >> 8); // check sign bit
-
 
146
}
-
 
147
 
-
 
148
// -----------------------------------------------------------------------
-
 
149
void delay_ms(uint16_t w) {
-
 
150
  uint16_t t_stop = setDelay(w);
-
 
151
  while (!checkDelay(t_stop))
-
 
152
    ;
-
 
153
}
-
 
154
 
-
 
155
// -----------------------------------------------------------------------
203
// -----------------------------------------------------------------------
156
void delay_ms_with_adc_measurement(uint16_t w, uint8_t stop) {
204
uint16_t setDelay(uint16_t t) {