Subversion Repositories FlightCtrl

Rev

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

Rev 1868 Rev 1869
Line 214... Line 214...
214 
  LCD_Clear();
 214 
  LCD_Clear();
Line 215... Line 215...
215 
 
 215 
 
Line 216... Line 216...
216 
  I2CTimeout = 5000;
 216 
  I2CTimeout = 5000;
217 
 
 217 
 
218 
  while (1) {
 218 
  while (1) {
Line 219... Line -...
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 
      /*
 219 
    if (runFlightControl) { // control interval
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.
 220 
      runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
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 
        }
 - 
 
Line -... Line 221...
- 
 
 221 
 
- 
 
 222 
      DebugOut.Digital[0] &= ~DEBUG_MAINLOOP_TIMER;
- 
 
 223 
      DebugOut.Digital[1] &= ~DEBUG_MAINLOOP_TIMER;
- 
 
 224 
 
- 
 
 225 
      if (analogDataReady) {
- 
 
 226 
        J4HIGH;
271 
      } else {
 227 
        flight_control();
272 
        RED_OFF;
 228 
        J4LOW;
273 
      }
 229 
 
274 
 
 230 
        /*
- 
 
 231 
         * If the motors are running (MKFlags & MKFLAG_MOTOR_RUN in flight.c), transmit
- 
 
 232 
         * the throttle vector just computed. Otherwise, if motor test is engaged, transmit
Line -... Line 233...
- 
 
 233 
         * the test throttle vector. If no testing, stop all motors.
- 
 
 234 
         */
- 
 
 235 
        // Obsoleted.
- 
 
 236 
        // transmitMotorThrottleData();
- 
 
 237 
 
- 
 
 238 
        RED_OFF;
- 
 
 239 
 
- 
 
 240 
        /*
- 
 
 241 
         Does not belong here. Instead, external control should be ignored in
- 
 
 242 
         controlMixer if there was no new data from there for some time.
- 
 
 243 
         if(externalControlActive) externalControlActive--;
- 
 
 244 
         else {
- 
 
 245 
         externalControl.config = 0;
- 
 
 246 
         externalStickPitch = 0;
- 
 
 247 
         externalStickRoll = 0;
- 
 
 248 
         externalStickYaw = 0;
- 
 
 249 
         }
- 
 
 250 
         */
- 
 
 251 
 
- 
 
 252 
        /*
- 
 
 253 
         Does not belong here.
- 
 
 254 
         if(RC_Quality)  RC_Quality--;
- 
 
 255 
         */
- 
 
 256 
 
- 
 
 257 
        /* Does not belong here. Well since we are not supporting navi right now anyway, leave out.
- 
 
 258 
         #ifdef USE_NAVICTRL
- 
 
 259 
         if(NCDataOkay) {
- 
 
 260 
         if(--NCDataOkay == 0) // no data from NC
- 
 
 261 
         {  // set gps control sticks neutral
- 
 
 262 
         GPSStickPitch = 0;
- 
 
 263 
         GPSStickRoll = 0;
- 
 
 264 
         NCSerialDataOkay = 0;
- 
 
 265 
         }
- 
 
 266 
         }
275 
      // Allow Serial Data Transmit if motors must not updated or motors are not running
 267 
         #endif
- 
 
 268 
         */
- 
 
 269 
        if (!--I2CTimeout || missingMotor) { // try to reset the i2c if motor is missing ot timeout
- 
 
 270 
          RED_ON;
- 
 
 271 
          if (!I2CTimeout) {
- 
 
 272 
            I2C_Reset();
Line 276... Line -...
276 
      if (!runFlightControl || !(MKFlags & MKFLAG_MOTOR_RUN)) {
 - 
 
277 
        usart0_TransmitTxData();
 - 
 
278 
      }
 273 
            I2CTimeout = 5;
279 
 
 274 
          }
280 
      usart0_ProcessRxData();
 275 
        } else {
281 
 
 276 
          RED_OFF;
- 
 
 277 
        }
282 
      if (CheckDelay(timer)) {
 278 
 
Line -... Line 279...
- 
 
 279 
        // Allow Serial Data Transmit if motors must not updated or motors are not running
- 
 
 280 
        if (!runFlightControl || !(MKFlags & MKFLAG_MOTOR_RUN)) {
- 
 
 281 
          usart0_TransmitTxData();
- 
 
 282 
        } else {
- 
 
 283 
          DebugOut.Digital[1] |= DEBUG_MAINLOOP_TIMER;
- 
 
 284 
        }
- 
 
 285 
 
- 
 
 286 
        usart0_ProcessRxData();
- 
 
 287 
 
- 
 
 288 
        if (CheckDelay(timer)) {
283 
        if (UBat <= UBAT_AT_5V) {
 289 
          if (UBat <= UBAT_AT_5V) {
284 
          // Do nothing. The voltage on the input side of the regulator is <5V;
 290 
            // Do nothing. The voltage on the input side of the regulator is <5V;
285 
          // we must be running off USB power. Keep it quiet.
 291 
            // we must be running off USB power. Keep it quiet.
286 
        } else if (UBat < staticParams.LowVoltageWarning) {
 292 
          } else if (UBat < staticParams.LowVoltageWarning) {
287 
          beepBatteryAlarm();
 293 
            beepBatteryAlarm();
- 
 
 294 
          }
- 
 
 295 
 
288 
        }
 296 
#ifdef USE_NAVICTRL
289 
 
 - 
 
290 
#ifdef USE_NAVICTRL
 - 
 
Line 291... Line 297...
291 
        SPI_StartTransmitPacket();
 297 
          SPI_StartTransmitPacket();
292 
        SendSPI = 4;
 298 
          SendSPI = 4;
293 
#endif
 299 
#endif
294 
        timer = SetDelay(20); // every 20 ms
 300 
          timer = SetDelay(20); // every 20 ms
295 
      }
 301 
        }
296 
      output_update();
 302 
        output_update();
297 
    }
 303 
      }
298 
 
 304 
 
299 
#ifdef USE_NAVICTRL
 305 
#ifdef USE_NAVICTRL
- 
 
 306 
      if(!SendSPI) {
300 
    if(!SendSPI) {
 307 
        // 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.
 308 
        // within the SPI_TransmitByte() routine the value is set to 4.
302 
      // within the SPI_TransmitByte() routine the value is set to 4.
 309 
        // I.e. the SPI_TransmitByte() is called at a rate of 9.765 kHz/4= 2441.25 Hz,