Subversion Repositories FlightCtrl

Rev

Rev 2088 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 2088 Rev 2097
1 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 - 
 
2 
// + Copyright (c) 04.2007 Holger Buss
 - 
 
3 
// + Nur f�r den privaten Gebrauch
 - 
 
4 
// + www.MikroKopter.com
 - 
 
5 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 - 
 
6 
// + Es gilt f�r das gesamte Projekt (Hardware, Software, Bin�rfiles, Sourcecode und Dokumentation),
 - 
 
7 
// + dass eine Nutzung (auch auszugsweise) nur f�r den privaten (nicht-kommerziellen) Gebrauch zul�ssig ist.
 - 
 
8 
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
 - 
 
9 
// + bzgl. der Nutzungsbedingungen aufzunehmen.
 - 
 
10 
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Best�ckung und Verkauf von Platinen oder Baus�tzen,
 - 
 
11 
// + Verkauf von Luftbildaufnahmen, usw.
 - 
 
12 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 - 
 
13 
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder ver�ffentlicht,
 - 
 
14 
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright m�ssen dann beiliegen
 - 
 
15 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 - 
 
16 
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
 - 
 
17 
// + auf anderen Webseiten oder sonstigen Medien ver�ffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
 - 
 
18 
// + eindeutig als Ursprung verlinkt werden
 - 
 
19 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 - 
 
20 
// + Keine Gew�hr auf Fehlerfreiheit, Vollst�ndigkeit oder Funktion
 - 
 
21 
// + Benutzung auf eigene Gefahr
 - 
 
22 
// + Wir �bernehmen keinerlei Haftung f�r direkte oder indirekte Personen- oder Sachsch�den
 - 
 
23 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 - 
 
24 
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
 - 
 
25 
// + mit unserer Zustimmung zul�ssig
 - 
 
26 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 - 
 
27 
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
 - 
 
28 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 - 
 
29 
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
 - 
 
30 
// + this list of conditions and the following disclaimer.
 - 
 
31 
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
 - 
 
32 
// +     from this software without specific prior written permission.
 - 
 
33 
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
 - 
 
34 
// +     for non-commercial use (directly or indirectly)
 - 
 
35 
// +     Commercial use (for example: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
 - 
 
36 
// +     with our written permission
 - 
 
37 
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
 - 
 
38 
// +     clearly linked as origin
 - 
 
39 
// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
 - 
 
40 
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 - 
 
41 
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 - 
 
42 
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 - 
 
43 
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 - 
 
44 
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 - 
 
45 
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 - 
 
46 
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 - 
 
47 
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 - 
 
48 
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 - 
 
49 
// +  POSSIBILITY OF SUCH DAMAGE.
 - 
 
50 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 - 
 
51 
#include <inttypes.h>
 1 
#include <inttypes.h>
52 
#include <avr/io.h>
 2 
#include <avr/io.h>
53 
#include <avr/interrupt.h>
 3 
#include <avr/interrupt.h>
54 
#include "eeprom.h"
 4 
#include "eeprom.h"
55 
#include "analog.h"
 5 
#include "analog.h"
56 
#include "controlMixer.h"
 6 
#include "controlMixer.h"
57 
 
 7 
 
58 
#include "timer0.h"
 8 
#include "timer0.h"
59 
#include "output.h"
 9 
#include "output.h"
60 
 
 10 
 
61 
#ifdef USE_MK3MAG
 11 
#ifdef USE_MK3MAG
62 
#include "mk3mag.h"
 12 
#include "mk3mag.h"
63 
#endif
 13 
#endif
64 
 
 14 
 
65 
volatile uint32_t globalMillisClock = 0;
 15 
volatile uint32_t globalMillisClock = 0;
66 
volatile uint8_t  runFlightControl = 0;
 16 
volatile uint8_t  runFlightControl = 0;
67 
volatile uint16_t beepTime = 0;
 17 
volatile uint16_t beepTime = 0;
68 
volatile uint16_t beepModulation = BEEP_MODULATION_NONE;
 18 
volatile uint16_t beepModulation = BEEP_MODULATION_NONE;
69 
 
 19 
 
70 
#ifdef USE_NAVICTRL
 20 
#ifdef USE_NAVICTRL
71 
volatile uint8_t SendSPI = 0;
 21 
volatile uint8_t SendSPI = 0;
72 
#endif
 22 
#endif
73 
 
 23 
 
74 
/*****************************************************
 24 
/*****************************************************
75 
 * Initialize Timer 0                  
 25 
 * Initialize Timer 0                  
76 
 *****************************************************/
 26 
 *****************************************************/
77 
// timer 0 is used for the PWM generation to control the offset voltage at the air pressure sensor
 27 
// timer 0 is used for the PWM generation to control the offset voltage at the air pressure sensor
78 
// Its overflow interrupt routine is used to generate the beep signal and the flight control motor update rate
 28 
// Its overflow interrupt routine is used to generate the beep signal and the flight control motor update rate
79 
void timer0_init(void) {
 29 
void timer0_init(void) {
80 
  uint8_t sreg = SREG;
 30 
  uint8_t sreg = SREG;
81 
 
 31 
 
82 
  // disable all interrupts before reconfiguration
 32 
  // disable all interrupts before reconfiguration
83 
  cli();
 33 
  cli();
84 
 
 34 
 
85 
  // Configure speaker port as output.
 35 
  // Configure speaker port as output.
86 
  if (boardRelease == 10) { // Speaker at PD2
 36 
  if (boardRelease == 10) { // Speaker at PD2
87 
    DDRD |= (1 << DDD2);
 37 
    DDRD |= (1 << DDD2);
88 
    PORTD &= ~(1 << PORTD2);
 38 
    PORTD &= ~(1 << PORTD2);
89 
  } else { // Speaker at PC7
 39 
  } else { // Speaker at PC7
90 
    DDRC |= (1 << DDC7);
 40 
    DDRC |= (1 << DDC7);
91 
    PORTC &= ~(1 << PORTC7);
 41 
    PORTC &= ~(1 << PORTC7);
92 
  }
 42 
  }
93 
 
 43 
 
94 
  // set PB3 and PB4 as output for the PWM used as offset for the pressure sensor
 44 
  // set PB3 and PB4 as output for the PWM used as offset for the pressure sensor
95 
  DDRB |= (1 << DDB4) | (1 << DDB3);
 45 
  DDRB |= (1 << DDB4) | (1 << DDB3);
96 
  PORTB &= ~((1 << PORTB4) | (1 << PORTB3));
 46 
  PORTB &= ~((1 << PORTB4) | (1 << PORTB3));
97 
 
 47 
 
98 
  // Timer/Counter 0 Control Register A
 48 
  // Timer/Counter 0 Control Register A
99 
 
 49 
 
100 
  // Waveform Generation Mode is Fast PWM (Bits WGM02 = 0, WGM01 = 1, WGM00 = 1)
 50 
  // Waveform Generation Mode is Fast PWM (Bits WGM02 = 0, WGM01 = 1, WGM00 = 1)
101 
  // Clear OC0A on Compare Match, set OC0A at BOTTOM, noninverting PWM (Bits COM0A1 = 1, COM0A0 = 0)
 51 
  // Clear OC0A on Compare Match, set OC0A at BOTTOM, noninverting PWM (Bits COM0A1 = 1, COM0A0 = 0)
102 
  // Clear OC0B on Compare Match, set OC0B at BOTTOM, (Bits COM0B1 = 1, COM0B0 = 0)
 52 
  // Clear OC0B on Compare Match, set OC0B at BOTTOM, (Bits COM0B1 = 1, COM0B0 = 0)
103 
  TCCR0A &= ~((1 << COM0A0) | (1 << COM0B0));
 53 
  TCCR0A &= ~((1 << COM0A0) | (1 << COM0B0));
104 
  TCCR0A |= (1 << COM0A1) | (1 << COM0B1) | (1 << WGM01) | (1 << WGM00);
 54 
  TCCR0A |= (1 << COM0A1) | (1 << COM0B1) | (1 << WGM01) | (1 << WGM00);
105 
 
 55 
 
106 
  // Timer/Counter 0 Control Register B
 56 
  // Timer/Counter 0 Control Register B
107 
  // set clock divider for timer 0 to SYSCLOCK/8 = 20MHz/8 = 2.5MHz
 57 
  // set clock divider for timer 0 to SYSCLOCK/8 = 20MHz/8 = 2.5MHz
108 
  // i.e. the timer increments from 0x00 to 0xFF with an update rate of 2.5 MHz
 58 
  // i.e. the timer increments from 0x00 to 0xFF with an update rate of 2.5 MHz
109 
  // hence the timer overflow interrupt frequency is 2.5 MHz/256 = 9.765 kHz
 59 
  // hence the timer overflow interrupt frequency is 2.5 MHz/256 = 9.765 kHz
110 
 
 60 
 
111 
  // divider 8 (Bits CS02 = 0, CS01 = 1, CS00 = 0)
 61 
  // divider 8 (Bits CS02 = 0, CS01 = 1, CS00 = 0)
112 
  TCCR0B &= ~((1 << FOC0A) | (1 << FOC0B) | (1 << WGM02));
 62 
  TCCR0B &= ~((1 << FOC0A) | (1 << FOC0B) | (1 << WGM02));
113 
  TCCR0B = (TCCR0B & 0xF8) | (0 << CS02) | (1 << CS01) | (0 << CS00);
 63 
  TCCR0B = (TCCR0B & 0xF8) | (0 << CS02) | (1 << CS01) | (0 << CS00);
114 
 
 64 
 
115 
  // initialize the Output Compare Register A & B used for PWM generation on port PB3 & PB4
 65 
  // initialize the Output Compare Register A & B used for PWM generation on port PB3 & PB4
116 
  OCR0A = 0; // for PB3
 66 
  OCR0A = 0; // for PB3
117 
  OCR0B = 120; // for PB4
 67 
  OCR0B = 120; // for PB4
118 
 
 68 
 
119 
  // init Timer/Counter 0 Register
 69 
  // init Timer/Counter 0 Register
120 
  TCNT0 = 0;
 70 
  TCNT0 = 0;
121 
 
 71 
 
122 
  // Timer/Counter 0 Interrupt Mask Register
 72 
  // Timer/Counter 0 Interrupt Mask Register
123 
  // enable timer overflow interrupt only
 73 
  // enable timer overflow interrupt only
124 
  TIMSK0 &= ~((1 << OCIE0B) | (1 << OCIE0A));
 74 
  TIMSK0 &= ~((1 << OCIE0B) | (1 << OCIE0A));
125 
  TIMSK0 |= (1 << TOIE0);
 75 
  TIMSK0 |= (1 << TOIE0);
126 
 
 76 
 
127 
  SREG = sreg;
 77 
  SREG = sreg;
128 
}
 78 
}
129 
 
 79 
 
130 
/*****************************************************/
 80 
/*****************************************************/
131 
/*          Interrupt Routine of Timer 0             */
 81 
/*          Interrupt Routine of Timer 0             */
132 
/*****************************************************/
 82 
/*****************************************************/
133 
ISR(TIMER0_OVF_vect) { // 9765.625 Hz
 83 
ISR(TIMER0_OVF_vect) { // 9765.625 Hz
134 
  static uint8_t cnt_1ms = 1, cnt = 0;
 84 
  static uint8_t cnt_1ms = 1, cnt = 0;
135 
  uint8_t beeperOn = 0;
 85 
  uint8_t beeperOn = 0;
136 
 
 86 
 
137 
#ifdef USE_NAVICTRL
 87 
#ifdef USE_NAVICTRL
138 
  if(SendSPI) SendSPI--; // if SendSPI is 0, the transmit of a byte via SPI bus to and from The Navicontrol is done
 88 
  if(SendSPI) SendSPI--; // if SendSPI is 0, the transmit of a byte via SPI bus to and from The Navicontrol is done
139 
#endif
 89 
#endif
140 
 
 90 
 
141 
  if (!cnt--) { // every 10th run (9.765625kHz/10 = 976.5625Hz)
 91 
  if (!cnt--) { // every 10th run (9.765625kHz/10 = 976.5625Hz)
142 
    cnt = 9;
 92 
    cnt = 9;
143 
    cnt_1ms ^= 1;
 93 
    cnt_1ms ^= 1;
144 
    if (!cnt_1ms) {
 94 
    if (!cnt_1ms) {
145 
      if (runFlightControl == 1)
 95 
      if (runFlightControl == 1)
146 
        debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER;
 96 
        debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER;
147 
      else
 97 
      else
148 
        debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER;
 98 
        debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER;
149 
      runFlightControl = 1; // every 2nd run (976.5625 Hz/2 = 488.28125 Hz)
 99 
      runFlightControl = 1; // every 2nd run (976.5625 Hz/2 = 488.28125 Hz)
150 
    }
 100 
    }
151 
    globalMillisClock++; // increment millisecond counter
 101 
    globalMillisClock++; // increment millisecond counter
152 
  }
 102 
  }
153 
 
 103 
 
154 
  // beeper on if duration is not over
 104 
  // beeper on if duration is not over
155 
  if (beepTime) {
 105 
  if (beepTime) {
156 
    beepTime--; // decrement BeepTime
 106 
    beepTime--; // decrement BeepTime
157 
    if (beepTime & beepModulation)
 107 
    if (beepTime & beepModulation)
158 
      beeperOn = 1;
 108 
      beeperOn = 1;
159 
    else
 109 
    else
160 
      beeperOn = 0;
 110 
      beeperOn = 0;
161 
  } else { // beeper off if duration is over
 111 
  } else { // beeper off if duration is over
162 
    beeperOn = 0;
 112 
    beeperOn = 0;
163 
    beepModulation = BEEP_MODULATION_NONE;
 113 
    beepModulation = BEEP_MODULATION_NONE;
164 
  }
 114 
  }
165 
 
 115 
 
166 
  if (beeperOn) {
 116 
  if (beeperOn) {
167 
    // set speaker port to high.
 117 
    // set speaker port to high.
168 
    if (boardRelease == 10)
 118 
    if (boardRelease == 10)
169 
      PORTD |= (1 << PORTD2); // Speaker at PD2
 119 
      PORTD |= (1 << PORTD2); // Speaker at PD2
170 
    else
 120 
    else
171 
      PORTC |= (1 << PORTC7); // Speaker at PC7
 121 
      PORTC |= (1 << PORTC7); // Speaker at PC7
172 
  } else { // beeper is off
 122 
  } else { // beeper is off
173 
    // set speaker port to low
 123 
    // set speaker port to low
174 
    if (boardRelease == 10)
 124 
    if (boardRelease == 10)
175 
      PORTD &= ~(1 << PORTD2);// Speaker at PD2
 125 
      PORTD &= ~(1 << PORTD2);// Speaker at PD2
176 
    else
 126 
    else
177 
      PORTC &= ~(1 << PORTC7);// Speaker at PC7
 127 
      PORTC &= ~(1 << PORTC7);// Speaker at PC7
178 
  }
 128 
  }
179 
 
 129 
 
180 
#ifdef USE_MK3MAG
 130 
#ifdef USE_MK3MAG
181 
  // update compass value if this option is enabled in the settings
 131 
  // update compass value if this option is enabled in the settings
182 
  if (staticParams.bitConfig & CFG_COMPASS_ENABLED) {
 132 
  if (staticParams.bitConfig & CFG_COMPASS_ENABLED) {
183 
    MK3MAG_periodicTask(); // read out mk3mag pwm
 133 
    MK3MAG_periodicTask(); // read out mk3mag pwm
184 
  }
 134 
  }
185 
#endif
 135 
#endif
186 
}
 136 
}
187 
 
 137 
 
188 
// -----------------------------------------------------------------------
 138 
// -----------------------------------------------------------------------
189 
uint16_t setDelay(uint16_t t) {
 139 
uint16_t setDelay(uint16_t t) {
190 
  return (globalMillisClock + t - 1);
 140 
  return (globalMillisClock + t - 1);
191 
}
 141 
}
192 
 
 142 
 
193 
// -----------------------------------------------------------------------
 143 
// -----------------------------------------------------------------------
194 
int8_t checkDelay(uint16_t t) {
 144 
int8_t checkDelay(uint16_t t) {
195 
  return (((t - globalMillisClock) & 0x8000) >> 8); // check sign bit
 145 
  return (((t - globalMillisClock) & 0x8000) >> 8); // check sign bit
196 
}
 146 
}
197 
 
 147 
 
198 
// -----------------------------------------------------------------------
 148 
// -----------------------------------------------------------------------
199 
void delay_ms(uint16_t w) {
 149 
void delay_ms(uint16_t w) {
200 
  uint16_t t_stop = setDelay(w);
 150 
  uint16_t t_stop = setDelay(w);
201 
  while (!checkDelay(t_stop))
 151 
  while (!checkDelay(t_stop))
202 
    ;
 152 
    ;
203 
}
 153 
}
204 
 
 154 
 
205 
// -----------------------------------------------------------------------
 155 
// -----------------------------------------------------------------------
206 
void delay_ms_with_adc_measurement(uint16_t w, uint8_t stop) {
 156 
void delay_ms_with_adc_measurement(uint16_t w, uint8_t stop) {
207 
  uint16_t t_stop;
 157 
  uint16_t t_stop;
208 
  t_stop = setDelay(w);
 158 
  t_stop = setDelay(w);
209 
  while (!checkDelay(t_stop)) {
 159 
  while (!checkDelay(t_stop)) {
210 
        if (analogDataReady) {
 160 
        if (analogDataReady) {
211 
          analog_update();
 161 
          analog_update();
212 
          startAnalogConversionCycle();
 162 
          startAnalogConversionCycle();
213 
        }
 163 
        }
214 
  }
 164 
  }
215 
  if (stop) {
 165 
  if (stop) {
216 
  // Wait for new samples to get prepared but do not restart AD conversion after that!
 166 
  // Wait for new samples to get prepared but do not restart AD conversion after that!
217 
  // Caller MUST to that.
 167 
  // Caller MUST to that.
218 
        while (!analogDataReady);
 168 
        while (!analogDataReady);
219 
  }
 169 
  }
220 
}
 170 
}
221 
 
 171