Subversion Repositories FlightCtrl

Rev

Rev 2097 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
1612 dongfang 1
#include <inttypes.h>
2
#include <avr/io.h>
3
#include <avr/interrupt.h>
2189 - 4
//#include "eeprom.h"
5
#include "profiler.h"
6
#include "controlMixer.h"
7
#include "configuration.h"
1612 dongfang 8
#include "analog.h"
1908 - 9
#include "timer0.h"
2189 - 10
#include "debug.h"
11
#include "beeper.h"
1869 - 12
#include "output.h"
2189 - 13
#include "commands.h"
14
#include "flight.h"
15
#include "uart0.h"
16
#include "twimaster.h"
1775 - 17
 
2052 - 18
#ifdef USE_MK3MAG
1612 dongfang 19
#include "mk3mag.h"
20
#endif
21
 
2189 - 22
#define MILLIS_DIVIDER 10
23
 
24
volatile uint32_t jiffiesClock;
25
volatile uint32_t millisClock;
26
volatile uint8_t  loopJiffiesClock;
27
volatile uint16_t beepTime;
1908 - 28
volatile uint16_t beepModulation = BEEP_MODULATION_NONE;
1612 dongfang 29
 
2189 - 30
volatile uint8_t flightControlStatus;
1612 dongfang 31
 
32
/*****************************************************
33
 * Initialize Timer 0                  
34
 *****************************************************/
35
// timer 0 is used for the PWM generation to control the offset voltage at the air pressure sensor
36
// Its overflow interrupt routine is used to generate the beep signal and the flight control motor update rate
37
void timer0_init(void) {
2189 - 38
        uint8_t sreg = SREG;
1612 dongfang 39
 
2189 - 40
        // disable all interrupts before reconfiguration
41
        cli();
1612 dongfang 42
 
2189 - 43
        // Configure speaker port as output.
44
        if (boardRelease == 10) { // Speaker at PD2
45
                DDRD |= (1 << DDD2);
46
                PORTD &= ~(1 << PORTD2);
47
        } else { // Speaker at PC7
48
                DDRC |= (1 << DDC7);
49
                PORTC &= ~(1 << PORTC7);
50
        }
1612 dongfang 51
 
2189 - 52
        // set PB3 and PB4 as output for the PWM used as offset for the pressure sensor
53
        DDRB |= (1 << DDB4) | (1 << DDB3);
54
        PORTB &= ~((1 << PORTB4) | (1 << PORTB3));
1612 dongfang 55
 
2189 - 56
        // Timer/Counter 0 Control Register A
1612 dongfang 57
 
2189 - 58
        // Waveform Generation Mode is Fast PWM (Bits WGM02 = 0, WGM01 = 1, WGM00 = 1)
59
        // Clear OC0A on Compare Match, set OC0A at BOTTOM, noninverting PWM (Bits COM0A1 = 1, COM0A0 = 0)
60
        // Clear OC0B on Compare Match, set OC0B at BOTTOM, (Bits COM0B1 = 1, COM0B0 = 0)
61
        TCCR0A &= ~((1 << COM0A0) | (1 << COM0B0));
62
        TCCR0A |= (1 << COM0A1) | (1 << COM0B1) | (1 << WGM01) | (1 << WGM00);
1612 dongfang 63
 
2189 - 64
        // Timer/Counter 0 Control Register B
65
        // set clock divider for timer 0 to SYSCLOCK/8 = 20MHz/8 = 2.5MHz
66
        // i.e. the timer increments from 0x00 to 0xFF with an update rate of 2.5 MHz
67
        // hence the timer overflow interrupt frequency is 2.5 MHz/256 = 9.765 kHz
68
        // divider 8 (Bits CS02 = 0, CS01 = 1, CS00 = 0)
69
        TCCR0B &= ~((1 << FOC0A) | (1 << FOC0B) | (1 << WGM02));
70
        TCCR0B = (TCCR0B & 0xF8) | (0 << CS02) | (1 << CS01) | (0 << CS00);
1612 dongfang 71
 
2189 - 72
        // initialize the Output Compare Register A & B used for PWM generation on port PB3 & PB4
73
        OCR0A = 0; // for PB3
74
        OCR0B = 120; // for PB4
1612 dongfang 75
 
2189 - 76
        // init Timer/Counter 0 Register
77
        TCNT0 = 0;
1612 dongfang 78
 
2189 - 79
        // Timer/Counter 0 Interrupt Mask Register
80
        // enable timer overflow interrupt only
81
        TIMSK0 &= ~((1 << OCIE0B) | (1 << OCIE0A));
82
        TIMSK0 |= (1 << TOIE0);
1612 dongfang 83
 
2189 - 84
        SREG = sreg;
85
}
1821 - 86
 
2189 - 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();
115
                analog_updateControlData();
116
                flight_control();
117
                output_applyMulticopterMixer();
118
                I2C_start(TWI_STATE_MOTOR_TX);
119
                J4LOW;
120
        }
121
 
122
        flightControlStatus = NOT_RUNNING;
1612 dongfang 123
}
124
 
125
/*****************************************************/
126
/*          Interrupt Routine of Timer 0             */
127
/*****************************************************/
2189 - 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;
1821 - 134
 
2189 - 135
        jiffiesClock++;
136
        loopJiffiesClock++;
137
        // profiler_scoreTimerHit();
1821 - 138
 
2189 - 139
        sei();
140
 
141
        if (!--millisDivider) {
142
                millisClock++;
143
                millisDivider = MILLIS_DIVIDER;
144
        }
145
 
146
        if (!--controlLoopDivider) {
147
            //sei();
148
                controlLoopDivider= CONTROLLOOP_DIVIDER;
149
                runFlightControlTask();
150
                //cli();
151
        }
152
 
153
    if (!--serialLoopDivider) {
154
        //sei();
155
        serialLoopDivider= SERIALLOOP_DIVIDER;
156
        // Allow serial data transmission if there is still time, or if we are not flying anyway.
157
        usart0_transmitTxData();
158
        usart0_processRxData();
159
        //cli();
1872 - 160
    }
1821 - 161
 
2189 - 162
    if (!--outputLoopDivider) {
163
        //sei();
164
        outputLoopDivider= OUTPUTLOOP_DIVIDER;
165
        output_update();
166
        //cli();
167
    }
1821 - 168
 
2189 - 169
        // beeper on if duration is not over
170
        if (beepTime) {
171
                beepTime--; // decrement BeepTime
172
                if (beepTime & beepModulation)
173
                        beeperOn = 1;
174
                else
175
                        beeperOn = 0;
176
        } else { // beeper off if duration is over
177
                beeperOn = 0;
178
                beepModulation = BEEP_MODULATION_NONE;
179
        }
1821 - 180
 
2189 - 181
        if (beeperOn) {
182
                // set speaker port to high.
183
                if (boardRelease == 10)
184
                        PORTD |= (1 << PORTD2); // Speaker at PD2
185
                else
186
                        PORTC |= (1 << PORTC7); // Speaker at PC7
187
        } else { // beeper is off
188
                // set speaker port to low
189
                if (boardRelease == 10)
190
                        PORTD &= ~(1 << PORTD2);// Speaker at PD2
191
                else
192
                        PORTC &= ~(1 << PORTC7);// Speaker at PC7
193
        }
194
 
2052 - 195
#ifdef USE_MK3MAG
2189 - 196
        // update compass value if this option is enabled in the settings
197
        if (staticParams.bitConfig & CFG_COMPASS_ENABLED) {
198
                MK3MAG_periodicTask(); // read out mk3mag pwm
199
        }
1612 dongfang 200
#endif
201
}
202
 
203
// -----------------------------------------------------------------------
1887 - 204
uint16_t setDelay(uint16_t t) {
2189 - 205
        return (millisClock + t - 1);
1612 dongfang 206
}
207
 
208
// -----------------------------------------------------------------------
1887 - 209
int8_t checkDelay(uint16_t t) {
2189 - 210
        return (((t - millisClock) & 0x8000) >> 8); // check sign bit
1612 dongfang 211
}
212
 
213
// -----------------------------------------------------------------------
1887 - 214
void delay_ms(uint16_t w) {
2189 - 215
        uint16_t t_stop = setDelay(w);
216
        while (!checkDelay(t_stop))
217
                ;
1612 dongfang 218
}