Subversion Repositories FlightCtrl

Rev

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

Rev 1805 Rev 1821
Line 75... Line 75...
75 
#ifdef USE_MK3MAG
 75 
#ifdef USE_MK3MAG
76 
#include "mk3mag.h"
 76 
#include "mk3mag.h"
77 
#endif
 77 
#endif
78 
#include "eeprom.h"
 78 
#include "eeprom.h"
Line 79... Line 79...
79 
 
 79 
 
80 
int16_t main (void) {
 80 
int16_t main(void) {
Line 81... Line 81...
81 
  uint16_t timer;
 81 
        uint16_t timer;
82 
 
 82 
 
Line 83... Line 83...
83 
  // disable interrupts global
 83 
        // disable interrupts global
84 
  cli();
 84 
        cli();
85 
 
 85 
 
86 
  // analyze hardware environment
 86 
        // analyze hardware environment
87 
  CPUType = getCPUType();
 87 
        CPUType = getCPUType();
88 
  BoardRelease = getBoardRelease();
 88 
        BoardRelease = getBoardRelease();
89 
 
 89 
 
90 
  // disable watchdog
 90 
        // disable watchdog
91 
  MCUSR &=~(1<<WDRF);
 91 
        MCUSR &= ~(1 << WDRF);
92 
  WDTCSR |= (1<<WDCE)|(1<<WDE);
 92 
        WDTCSR |= (1 << WDCE) | (1 << WDE);
93 
  WDTCSR = 0;
 93 
        WDTCSR = 0;
94 
 
 94 
 
95 
  // PPM_in[CH_THROTTLE] = 0;
 95 
        // PPM_in[CH_THROTTLE] = 0;
96 
  // Why??? They are already initialized to 0.
 96 
        // Why??? They are already initialized to 0.
97 
  // stickPitch = stickRoll = stickYaw = 0;
 97 
        // stickPitch = stickRoll = stickYaw = 0;
98 
 
 98 
 
99 
  RED_OFF;
 99 
        RED_OFF;
100 
 
 100 
 
101 
  // initalize modules
 101 
        // initalize modules
102 
  output_init();
 102 
        output_init();
103 
  timer0_init();
 103 
        timer0_init();
- 
 
 104 
        timer2_init();
104 
  timer2_init();
 105 
        usart0_Init();
105 
  usart0_Init();
 106 
        if (CPUType == ATMEGA644P)
106 
  if(CPUType == ATMEGA644P) usart1_Init();
 107 
                usart1_Init();
107 
  RC_Init();
 108 
        RC_Init();
108 
  analog_init();
 109 
        analog_init();
109 
  I2C_init();
 110 
        I2C_init();
110 
#ifdef USE_NAVICTRL
 111 
#ifdef USE_NAVICTRL
111 
  SPI_MasterInit();
 112 
        SPI_MasterInit();
112 
#endif
 113 
#endif
113 
#ifdef USE_MK3MAG
 114 
#ifdef USE_MK3MAG
114 
  MK3MAG_Init();
 115 
        MK3MAG_Init();
115 
#endif
 116 
#endif
116 
 
 117 
 
117 
  // enable interrupts global
 118 
        // enable interrupts global
118 
  sei();
 119 
        sei();
119 
 
 120 
 
120 
  printf("\n\r===================================");
 121 
        printf("\n\r===================================");
121 
  printf("\n\rFlightControl");
 122 
        printf("\n\rFlightControl");
122 
  printf("\n\rHardware: Custom");
 123 
        printf("\n\rHardware: Custom");
123 
  printf("\r\n     CPU: Atmega644");
 124 
        printf("\r\n     CPU: Atmega644");
124 
  if(CPUType == ATMEGA644P)
 125 
        if (CPUType == ATMEGA644P)
125 
    printf("p");
 126 
                printf("p");
126 
  printf("\n\rSoftware: V%d.%d%c",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a');
 127 
        printf("\n\rSoftware: V%d.%d%c",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a');
127 
  printf("\n\r===================================");
 128 
        printf("\n\r===================================");
128 
 
 129 
 
129 
  // Parameter Set handling
 130 
        // Parameter Set handling
130 
  ParamSet_Init();
 131 
        ParamSet_Init();
131 
 
 132 
 
132 
  // Wait for a short time (otherwise the RC channel check won't work below)
 133 
        // Wait for a short time (otherwise the RC channel check won't work below)
133 
  // timer = SetDelay(500);
 134 
        // timer = SetDelay(500);
134 
  // while(!CheckDelay(timer));
 135 
        // while(!CheckDelay(timer));
135 
 
 136 
 
136 
  // Instead, while away the time by flashing the 2 outputs:
 137 
        // Instead, while away the time by flashing the 2 outputs:
137 
  // First J16, then J17. Makes it easier to see which is which.
 138 
        // First J16, then J17. Makes it easier to see which is which.
138 
  timer = SetDelay(200);
 139 
        timer = SetDelay(200);
139 
  OUTPUT_SET(0,1);
 140 
        OUTPUT_SET(0,1);
- 
 
 141 
        GRN_OFF;
140 
  GRN_OFF;
 142 
        RED_ON;
141 
  RED_ON;
 143 
        while (!CheckDelay(timer))
142 
  while(!CheckDelay(timer));
 144 
                ;
143 
 
 145 
 
144 
  timer = SetDelay(200);
 146 
        timer = SetDelay(200);
145 
  OUTPUT_SET(0,0);
 147 
        OUTPUT_SET(0,0);
146 
  OUTPUT_SET(1,1);
 148 
        OUTPUT_SET(1,1);
- 
 
 149 
        RED_OFF;
147 
  RED_OFF;
 150 
        GRN_ON;
148 
  GRN_ON;
 151 
        while (!CheckDelay(timer))
149 
  while(!CheckDelay(timer));
 152 
                ;
- 
 
 153 
 
150 
 
 154 
        timer = SetDelay(200);
151 
  timer = SetDelay(200);
 155 
        while (!CheckDelay(timer))
152 
  while(!CheckDelay(timer));
 156 
                ;
153 
  OUTPUT_SET(1,0);
 157 
        OUTPUT_SET(1,0);
154 
 
 158 
 
155 
  twi_diagnostics();
 159 
        twi_diagnostics();
156 
 
 160 
 
157 
  printf("\n\r===================================");
 161 
        printf("\n\r===================================");
158 
 
 162 
 
159 
  /*
 163 
        /*
160 
  if(staticParams.GlobalConfig & CFG_HEIGHT_CONTROL)
 164 
         if(staticParams.GlobalConfig & CFG_HEIGHT_CONTROL)
161 
    {
 165 
         {
162 
      printf("\n\rCalibrating air pressure sensor..");
 166 
         printf("\n\rCalibrating air pressure sensor..");
163 
      timer = SetDelay(1000);
 167 
         timer = SetDelay(1000);
164 
      SearchAirPressureOffset();
 168 
         SearchAirPressureOffset();
165 
      while (!CheckDelay(timer));
 169 
         while (!CheckDelay(timer));
Line 166... Line 170...
166 
      printf("OK\n\r");
 170 
         printf("OK\n\r");
167 
    }
 171 
         }
168 
  */
 172 
         */
169 
 
 173 
 
170 
#ifdef USE_NAVICTRL
 174 
#ifdef USE_NAVICTRL
171 
  printf("\n\rSupport for NaviCtrl");
 175 
        printf("\n\rSupport for NaviCtrl");
172 
#ifdef USE_RC_DSL
 176 
#ifdef USE_RC_DSL
173 
  printf("\r\nSupport for DSL RC at 2nd UART");
 177 
        printf("\r\nSupport for DSL RC at 2nd UART");
174 
#endif
 178 
#endif
Line 175... Line 179...
175 
#ifdef USE_RC_SPECTRUM
 179 
#ifdef USE_RC_SPECTRUM
176 
  printf("\r\nSupport for SPECTRUM RC at 2nd UART");
 180 
        printf("\r\nSupport for SPECTRUM RC at 2nd UART");
177 
#endif
 181 
#endif
Line 178... Line 182...
178 
#endif
 182 
#endif
179 
 
 183 
 
180 
#ifdef USE_MK3MAG
 184 
#ifdef USE_MK3MAG
181 
  printf("\n\rSupport for MK3MAG Compass");
 185 
        printf("\n\rSupport for MK3MAG Compass");
Line 182... Line 186...
182 
#endif
 186 
#endif
Line 183... Line 187...
183 
 
 187 
 
184 
#if (defined (USE_MK3MAG))
 188 
#if (defined (USE_MK3MAG))
- 
 
 189 
        if(CPUType == ATMEGA644P) printf("\n\rSupport for GPS at 2nd UART");
- 
 
 190 
        else printf("\n\rSupport for GPS at 1st UART");
- 
 
 191 
#endif
- 
 
 192 
 
- 
 
 193 
        controlMixer_setNeutral();
- 
 
 194 
 
- 
 
 195 
        // Cal. attitude sensors and reset integrals.
- 
 
 196 
        attitude_setNeutral();
- 
 
 197 
 
- 
 
 198 
        Servo_On();
- 
 
 199 
 
- 
 
 200 
        // Init flight parameters
- 
 
 201 
        flight_setNeutral();
- 
 
 202 
 
- 
 
 203 
        // RED_OFF;
- 
 
 204 
 
- 
 
 205 
        beep(2000);
- 
 
 206 
 
- 
 
 207 
        printf("\n\rControl: ");
- 
 
 208 
        if (staticParams.GlobalConfig & CFG_HEADING_HOLD)
- 
 
 209 
                printf("HeadingHold");
- 
 
 210 
                else printf("Neutral (ACC-Mode)");
- 
 
 211 
 
- 
 
 212 
        printf("\n\n\r");
- 
 
 213 
 
- 
 
 214 
        LCD_Clear();
- 
 
 215 
 
- 
 
 216 
        I2CTimeout = 5000;
- 
 
 217 
 
- 
 
 218 
        while (1) {
- 
 
 219 
                if (runFlightControl && analogDataReady) { // control interval
- 
 
 220 
                        runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
- 
 
 221 
 
- 
 
 222 
                        J4HIGH;
- 
 
 223 
                        flight_control();
- 
 
 224 
                        J4LOW;
- 
 
 225 
 
- 
 
 226 
                        /*
- 
 
 227 
                         * If the motors are running (MKFlags & MKFLAG_MOTOR_RUN in flight.c), transmit
- 
 
 228 
                         * the throttle vector just computed. Otherwise, if motor test is engaged, transmit
- 
 
 229 
                         * the test throttle vector. If no testing, stop all motors.
- 
 
 230 
                         */
- 
 
 231 
                        // Obsoleted.
- 
 
 232 
                        // transmitMotorThrottleData();
- 
 
 233 
 
- 
 
 234 
                        RED_OFF;
- 
 
 235 
 
- 
 
 236 
                        /*
- 
 
 237 
                         Does not belong here. Instead, external control should be ignored in
- 
 
 238 
                         controlMixer if there was no new data from there for some time.
- 
 
 239 
                         if(externalControlActive) externalControlActive--;
- 
 
 240 
                         else {
- 
 
 241 
                         externalControl.config = 0;
- 
 
 242 
                         externalStickPitch = 0;
- 
 
 243 
                         externalStickRoll = 0;
- 
 
 244 
                         externalStickYaw = 0;
- 
 
 245 
                         }
- 
 
 246 
                         */
- 
 
 247 
 
- 
 
 248 
                        /*
- 
 
 249 
                         Does not belong here.
- 
 
 250 
                         if(RC_Quality)  RC_Quality--;
- 
 
 251 
                         */
- 
 
 252 
 
- 
 
 253 
                        /* Does not belong here. Well since we are not supporting navi right now anyway, leave out.
- 
 
 254 
                         #ifdef USE_NAVICTRL
- 
 
 255 
                         if(NCDataOkay) {
- 
 
 256 
                         if(--NCDataOkay == 0) // no data from NC
- 
 
 257 
                         {  // set gps control sticks neutral
- 
 
 258 
                         GPSStickPitch = 0;
- 
 
 259 
                         GPSStickRoll = 0;
- 
 
 260 
                         NCSerialDataOkay = 0;
- 
 
 261 
                         }
- 
 
 262 
                         }
- 
 
 263 
                         #endif
- 
 
 264 
                         */
- 
 
 265 
                        if (!--I2CTimeout || missingMotor) { // try to reset the i2c if motor is missing ot timeout
- 
 
 266 
                                RED_ON;
- 
 
 267 
                                if (!I2CTimeout) {
- 
 
 268 
                                        I2C_Reset();
- 
 
 269 
                                        I2CTimeout = 5;
- 
 
 270 
                                }
- 
 
 271 
                        } else {
- 
 
 272 
                                RED_OFF;
- 
 
 273 
                        }
- 
 
 274 
 
- 
 
 275 
                        // Allow Serial Data Transmit if motors must not updated or motors are not running
- 
 
 276 
                        if (!runFlightControl || !(MKFlags & MKFLAG_MOTOR_RUN)) {
- 
 
 277 
                                usart0_TransmitTxData();
- 
 
 278 
                        }
- 
 
 279 
 
- 
 
 280 
                        usart0_ProcessRxData();
Line 185... Line -...
185 
  if(CPUType == ATMEGA644P) printf("\n\rSupport for GPS at 2nd UART");
 - 
 
186 
  else                      printf("\n\rSupport for GPS at 1st UART");
 - 
 
187 
#endif
 - 
 
188 
 
 - 
 
189 
  controlMixer_setNeutral();
 - 
 
190 
 
 - 
 
191 
  // Cal. attitude sensors and reset integrals.
 - 
 
192 
  attitude_setNeutral();
 - 
 
193 
 
 - 
 
194 
  Servo_On();
 - 
 
195 
 
 - 
 
196 
  // Init flight parameters
 - 
 
197 
  flight_setNeutral();
 - 
 
198 
 
 - 
 
199 
  // RED_OFF;
 - 
 
200 
 
 - 
 
201 
  beep(2000);
 - 
 
202 
 
 - 
 
203 
  printf("\n\rControl: ");
 - 
 
204 
  if (staticParams.GlobalConfig & CFG_HEADING_HOLD) printf("HeadingHold");
 - 
 
205 
  else printf("Neutral (ACC-Mode)");
 - 
 
206 
 
 - 
 
207 
  printf("\n\n\r");
 - 
 
208 
 
 - 
 
209 
  LCD_Clear();
 - 
 
210 
 
 - 
 
211 
  I2CTimeout = 5000;
 - 
 
212 
 
 - 
 
213 
  while (1) {
 - 
 
214 
    if(runFlightControl && analogDataReady) {      // control interval
 - 
 
215 
      runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
 - 
 
216 
     
 - 
 
217 
      J4HIGH;
 - 
 
218 
      flight_control();
 - 
 
219 
      J4LOW;
 - 
 
220 
     
 - 
 
221 
      /*
 - 
 
222 
       * If the motors are running (MKFlags & MKFLAG_MOTOR_RUN in flight.c), transmit
 - 
 
223 
       * the throttle vector just computed. Otherwise, if motor test is engaged, transmit
 - 
 
224 
       * the test throttle vector. If no testing, stop all motors.
 - 
 
225 
       */
 - 
 
226 
      // Obsoleted.
 - 
 
227 
      // transmitMotorThrottleData();
 - 
 
228 
     
 - 
 
229 
      RED_OFF;
 - 
 
230 
     
 - 
 
231 
      /*
 - 
 
232 
        Does not belong here. Instead, external control should be ignored in
 - 
 
233 
        controlMixer if there was no new data from there for some time.
 - 
 
234 
      if(externalControlActive) externalControlActive--;
 - 
 
235 
      else {
 - 
 
236 
        externalControl.config = 0;
 - 
 
237 
        externalStickPitch = 0;
 - 
 
238 
        externalStickRoll = 0;
 - 
 
239 
        externalStickYaw = 0;
 - 
 
240 
      }
 - 
 
241 
      */
 - 
 
242 
     
 - 
 
243 
      /*
 - 
 
244 
        Does not belong here.
 - 
 
245 
        if(RC_Quality)  RC_Quality--;
 - 
 
246 
      */
 - 
 
247 
     
 - 
 
248 
      /* Does not belong here. Well since we are not supporting navi right now anyway, leave out.
 - 
 
249 
#ifdef USE_NAVICTRL
 - 
 
250 
      if(NCDataOkay) {
 - 
 
251 
        if(--NCDataOkay == 0) // no data from NC
 - 
 
252 
          {  // set gps control sticks neutral
 - 
 
253 
            GPSStickPitch = 0;
 - 
 
254 
            GPSStickRoll = 0;
 - 
 
255 
            NCSerialDataOkay = 0;
 - 
 
256 
          }
 - 
 
257 
      }
 - 
 
258 
#endif
 - 
 
259 
      */
 - 
 
260 
      if(!--I2CTimeout || missingMotor) { // try to reset the i2c if motor is missing ot timeout
 - 
 
261 
        RED_ON;
 - 
 
262 
        if(!I2CTimeout) {
 - 
 
263 
          I2C_Reset();
 - 
 
264 
          I2CTimeout = 5;
 - 
 
265 
        }
 - 
 
266 
      } else {
 - 
 
267 
        RED_OFF;
 - 
 
268 
      }
 - 
 
269 
     
 - 
 
270 
      // Allow Serial Data Transmit if motors must not updated or motors are not running
 - 
 
271 
      if( !runFlightControl || !(MKFlags & MKFLAG_MOTOR_RUN)) {
 - 
 
272 
        usart0_TransmitTxData();
 - 
 
273 
      }
 - 
 
274 
     
 - 
 
275 
      usart0_ProcessRxData();
 - 
 
276 
     
 281 
 
277 
      if(CheckDelay(timer)) {
 282 
                        if (CheckDelay(timer)) {
278 
        if (UBat <= UBAT_AT_5V) {
 283 
                                if (UBat <= UBAT_AT_5V) {
279 
          // Do nothing. The voltage on the input side of the regulator is <5V;
284 
                                        // Do nothing. The voltage on the input side of the regulator is <5V;
280 
          // we must be running off USB power. Keep it quiet.
 285 
                                        // we must be running off USB power. Keep it quiet.
281 
        } else if(UBat < staticParams.LowVoltageWarning) {
 286 
                                } else if (UBat < staticParams.LowVoltageWarning) {
282 
          beepBatteryAlarm();
 287 
                                        beepBatteryAlarm();
283 
        }
 288 
                                }
284 
       
 289 
 
285 
#ifdef USE_NAVICTRL
 290 
#ifdef USE_NAVICTRL
286 
        SPI_StartTransmitPacket();
 291 
                                SPI_StartTransmitPacket();
287 
        SendSPI = 4;
 292 
                                SendSPI = 4;
288 
#endif
 293 
#endif
289 
        timer = SetDelay(20); // every 20 ms
 294 
                                timer = SetDelay(20); // every 20 ms
290 
      }
 295 
                        }
291 
      output_update();
 296 
                        output_update();
292 
    }
 297 
                }
293 
   
 298 
 
294 
#ifdef USE_NAVICTRL
 299 
#ifdef USE_NAVICTRL
295 
    if(!SendSPI) {
 300 
                if(!SendSPI) {
296 
      // SendSPI is decremented in timer0.c with a rate of 9.765 kHz.
 301 
                        // SendSPI is decremented in timer0.c with a rate of 9.765 kHz.