WebSVN - FlightCtrl - Diff - Rev 1646 and 1775 - /branches/dongfang_FC

 #include "timer0.h"
 #include "uart0.h"
 #include "attitude.h"
 #include "rc.h"
 #include "externalControl.h"
+#include "output.h"
 #if defined (USE_MK3MAG)
 #include "ubx.h"
 #endif
 #define MK3MAG_ADDRESS 3
 #define FALSE   0
 #define TRUE    1
 //int8_t test __attribute__ ((section (".noinit")));
-uint8_t Request_VerInfo                 = FALSE;
+uint8_t request_VerInfo                 = FALSE;
-uint8_t Request_ExternalControl         = FALSE;
+uint8_t request_ExternalControl         = FALSE;
-uint8_t Request_Display                 = FALSE;
+uint8_t request_Display                 = FALSE;
-uint8_t Request_Display1                = FALSE;
+uint8_t request_Display1                = FALSE;
-uint8_t Request_DebugData               = FALSE;
+uint8_t request_DebugData               = FALSE;
-uint8_t Request_Data3D                  = FALSE;
+uint8_t request_Data3D                  = FALSE;
-uint8_t Request_DebugLabel              = 255;
+uint8_t request_DebugLabel              = 255;
-uint8_t Request_PPMChannels     = FALSE;
+uint8_t request_PPMChannels             = FALSE;
+uint8_t request_MotorTest               = FALSE;
+uint8_t request_variables               = FALSE;
 uint8_t Request_MotorTest               = FALSE;
 uint8_t DisplayLine = 0;
 volatile uint8_t txd_buffer[TXD_BUFFER_LEN];
     "GyroRoll(PID)   ",
     "GyroYaw         ", //5
     "GyroPitch(AC)   ",
     "GyroRoll(AC)    ",
     "GyroYaw(AC)     ",
-    "AccPitch        ",
+    "AccPitch (angle)",
-    "AccRoll         ", //10
+    "AccRoll (angle) ", //10
-    "CorrSumPitch    ",
+    "UBat            ",
-    "CorrSumRoll     ",
+    "Pitch Term      ",
-    "Pressure range  ",
+    "Roll Term       ",
-    "Pressure A/D    ",
+    "Yaw Term        ",
-    "Pressure Value  ", //15
+    "Throttle Term   ", //15
-    "Press. rangewidz",
+    "0th O Corr pitch",
-    "Press. init A/D ",
+    "0th O Corr roll ",
-    "FACTOR          ",
+    "DriftCompDelta P",
-    "throttleTerm    ",
+    "DriftCompDelta R",
-    "pitchTerm       ", //20
+    "ADPitchGyroOffs ", //20
-    "rollTerm        ",
+    "ADRollGyroOffs  ",
-    "yawTerm         ",
+    "M1              ",
-    "P Pitch         ",
+    "M2              ",
-    "I Pitch         ",
+    "M3              ",
-    "P+D Pitch       ", //25
+    "M4              ", //25
-    "Pitch acc noise ",
+    "ControlYaw      ",
-    "Roll acc noise  ",
+    "Acc Z           ",
-    "DynamicPitch    ",
+    "DriftCompPitch  ",
-    "DynamicRoll     ",
+    "DriftCompRoll   ",
     "Pitch Noise     ", //30
     "Roll Noise      "
   };
   txd_complete = FALSE;
   UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR)
+}
+// --------------------------------------------------------------------------
+// application example:
+// SendOutData('A', FC_ADDRESS, 2, (uint8_t *)&request_DebugLabel, sizeof(request_DebugLabel), label, 16);
+/*
+void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ...
+  va_list ap;
+  uint16_t txd_bufferIndex = 0;
+  uint8_t *currentBuffer;
+  uint8_t currentBufferIndex;
+  uint16_t lengthOfCurrentBuffer;
+  uint8_t shift = 0;
+  txd_buffer[txd_bufferIndex++] = '#';                  // Start character
+  txd_buffer[txd_bufferIndex++] = 'a' + addr;           // Address (a=0; b=1,...)
+  txd_buffer[txd_bufferIndex++] = cmd;                  // Command
+  va_start(ap, numofbuffers);
+  while(numofbuffers) {
+    currentBuffer = va_arg(ap, uint8_t*);
+    lengthOfCurrentBuffer = va_arg(ap, int);
+    currentBufferIndex = 0;
+    // Encode data: 3 bytes of data are encoded into 4 bytes,
+    // where the 2 most significant bits are both 0.
+    while(currentBufferIndex != lengthOfCurrentBuffer) {
+      if (!shift) txd_buffer[txd_bufferIndex] = 0;
+      txd_buffer[txd_bufferIndex]  |= currentBuffer[currentBufferIndex] >> (shift + 2);
+      txd_buffer[++txd_bufferIndex] = (currentBuffer[currentBufferIndex] << (4 - shift)) & 0b00111111;
+      shift += 2;
+      if (shift == 6) { shift=0; txd_bufferIndex++; }
+      currentBufferIndex++;
+    }
+  }
+  // If the number of data bytes was not divisible by 3, stuff
+  //  with 0 pseudodata  until length is again divisible by 3.
+  if (shift == 2) {
+    // We need to stuff with zero bytes at the end.
+    txd_buffer[txd_bufferIndex]  &= 0b00110000;
+    txd_buffer[++txd_bufferIndex] = 0;
+    shift = 4;
+  }
+  if (shift == 4) {
+    // We need to stuff with zero bytes at the end.
+    txd_buffer[txd_bufferIndex++] &= 0b00111100;
+    txd_buffer[txd_bufferIndex]    = 0;
+  }
+  va_end(ap);
+  AddCRC(pt); // add checksum after data block and initates the transmission
+}
+*/
 // --------------------------------------------------------------------------
 void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ...
   va_list ap;
   uint16_t pt = 0;
   uint8_t a,b,c;
   RxDataLen = ptrOut - 3;
+}
 // --------------------------------------------------------------------------
+void usart0_ProcessRxData(void) {
+  // We control the motorTestActive var from here: Count it down.
-void usart0_ProcessRxData(void) {
+  if (motorTestActive) motorTestActive--;
   // if data in the rxd buffer are not locked immediately return
-  if(!rxd_buffer_locked) return;
   if(!rxd_buffer_locked) return;
-  uint8_t tempchar1, tempchar2;
   uint8_t tempchar1, tempchar2;
   Decode64(); // decode data block in rxd_buffer
+  switch(rxd_buffer[1] - 'a') {
   switch(rxd_buffer[1] - 'a') {
   case FC_ADDRESS:
     switch(rxd_buffer[2]) {
 #ifdef USE_MK3MAG
     case 'K':// compass value
-      CompassHeading = ((Heading_t *)pRxData)->Heading;
+      compassHeading = ((Heading_t *)pRxData)->Heading;
-      CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180;
+      compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180;
       break;
-#endif
+#endif
-    case 't':// motor test
+    case 't': // motor test
-      if(RxDataLen > 20) //
-        {
+      if(RxDataLen > 20) {
-          memcpy(&motorTest[0], (uint8_t*)pRxData, sizeof(motorTest));
-        }
+        memcpy(&motorTest[0], (uint8_t*)pRxData, sizeof(motorTest));
-      else
-        {
+      } else {
-          memcpy(&motorTest[0], (uint8_t*)pRxData, 4);
+        memcpy(&motorTest[0], (uint8_t*)pRxData, 4);
-        }
       //Request_MotorTest = TRUE;
       motorTestActive = 255;
       externalControlActive = 255;
       break;
     case 'n':// "Get Mixer Table
       while(!txd_complete); // wait for previous frame to be sent
       SendOutData('N', FC_ADDRESS, 1, (uint8_t *) &Mixer, sizeof(Mixer));
       break;
-    case 'm':// "Set Mixer Table
-      if(pRxData[0] == EEMIXER_REVISION)
+    case 'm':// "Set Mixer Table
-        {
+      if(pRxData[0] == EEMIXER_REVISION) {
-          memcpy(&Mixer, (uint8_t*)pRxData, sizeof(Mixer));
+        memcpy(&Mixer, (uint8_t*)pRxData, sizeof(Mixer));
-          MixerTable_WriteToEEProm();
-          while(!txd_complete); // wait for previous frame to be sent
+        MixerTable_WriteToEEProm();
-          tempchar1 = 1;
-        }
+        while(!txd_complete); // wait for previous frame to be sent
-      else
+        tempchar1 = 1;
       } else {
-          tempchar1 = 0;
+        tempchar1 = 0;
+      }
       SendOutData('M', FC_ADDRESS,  1, &tempchar1, 1);
       break;
     case 'p': // get PPM channels
-      Request_PPMChannels = TRUE;
-      break;
+      request_PPMChannels = TRUE;
+      break;
     case 'q':// request settings
-      if(pRxData[0] == 0xFF)
+    case 'q':// request settings
       if(pRxData[0] == 0xFF) {
-          pRxData[0] = GetParamByte(PID_ACTIVE_SET);
+        pRxData[0] = GetParamByte(PID_ACTIVE_SET);
+      }
       //unsupported command received
       break;
     } // case FC_ADDRESS:
-  default: // any Slave Address
+  default: // any Slave Address
-    switch(rxd_buffer[2])
     switch(rxd_buffer[2]) {
       case 'a':// request for labels of the analog debug outputs
-        Request_DebugLabel = pRxData[0];
+        request_DebugLabel = pRxData[0];
-        if(Request_DebugLabel > 31) Request_DebugLabel = 31;
+        if(request_DebugLabel > 31) request_DebugLabel = 31;
         externalControlActive = 255;
         break;
       case 'h':// request for display columns
         externalControlActive = 255;
         RemoteKeys |= pRxData[0];
         if(RemoteKeys) DisplayLine = 0;
-        Request_Display = TRUE;
+        request_Display = TRUE;
         break;
       case 'l':// request for display columns
         externalControlActive = 255;
         MenuItem = pRxData[0];
-        Request_Display1 = TRUE;
+        request_Display1 = TRUE;
         break;
+      case 'v': // request for version and board release
+        request_VerInfo = TRUE;
+        break;
-      case 'v': // request for version and board release
+    case 'x':
-        Request_VerInfo = TRUE;
+      request_variables = TRUE;
-        break;
+      break;
       case 'g':// get external control data
-        Request_ExternalControl = TRUE;
+        request_ExternalControl = TRUE;
         break;
       case 'd': // request for the debug data
         DebugData_Interval = (uint16_t) pRxData[0] * 10;
-        if(DebugData_Interval > 0) Request_DebugData = TRUE;
+        if(DebugData_Interval > 0) request_DebugData = TRUE;
         break;
       case 'c': // request for the 3D data
 //---------------------------------------------------------------------------------------------
 void usart0_TransmitTxData(void) {
   if(!txd_complete) return;
-  if(Request_VerInfo && txd_complete) {
+  if(request_VerInfo && txd_complete) {
     SendOutData('V', FC_ADDRESS, 1, (uint8_t *) &UART_VersionInfo, sizeof(UART_VersionInfo));
-    Request_VerInfo = FALSE;
+    request_VerInfo = FALSE;
+  }
-  if(Request_Display && txd_complete) {
+  if(request_Display && txd_complete) {
     LCD_PrintMenu();
     SendOutData('H', FC_ADDRESS, 2, &DisplayLine, sizeof(DisplayLine), &DisplayBuff[DisplayLine * 20], 20);
     DisplayLine++;
     if(DisplayLine >= 4) DisplayLine = 0;
-    Request_Display = FALSE;
+    request_Display = FALSE;
+  }
-  if(Request_Display1 && txd_complete) {
+  if(request_Display1 && txd_complete) {
     LCD_PrintMenu();
     SendOutData('L', FC_ADDRESS, 3, &MenuItem, sizeof(MenuItem), &MaxMenuItem, sizeof(MaxMenuItem), DisplayBuff, sizeof(DisplayBuff));
-    Request_Display1 = FALSE;
+    request_Display1 = FALSE;
   }
-  if(Request_DebugLabel != 0xFF) { // Texte für die Analogdaten
+  if(request_DebugLabel != 0xFF) { // Texte für die Analogdaten
     uint8_t label[16]; // local sram buffer
-    memcpy_P(label, ANALOG_LABEL[Request_DebugLabel], 16); // read lable from flash to sram buffer
+    memcpy_P(label, ANALOG_LABEL[request_DebugLabel], 16); // read lable from flash to sram buffer
-    SendOutData('A', FC_ADDRESS, 2, (uint8_t *) &Request_DebugLabel, sizeof(Request_DebugLabel), label, 16);
+    SendOutData('A', FC_ADDRESS, 2, (uint8_t *) &request_DebugLabel, sizeof(request_DebugLabel), label, 16);
-    Request_DebugLabel = 0xFF;
+    request_DebugLabel = 0xFF;
   }
   if(ConfirmFrame && txd_complete) {   // Datensatz ohne CRC bestätigen
     SendOutData('B', FC_ADDRESS, 1, (uint8_t*)&ConfirmFrame, sizeof(ConfirmFrame));
     ConfirmFrame = 0;
   }
-  if(((DebugData_Interval && CheckDelay(DebugData_Timer)) || Request_DebugData) && txd_complete) {
+  if(((DebugData_Interval && CheckDelay(DebugData_Timer)) || request_DebugData) && txd_complete) {
     SendOutData('D', FC_ADDRESS, 1,(uint8_t *) &DebugOut, sizeof(DebugOut));
     DebugData_Timer = SetDelay(DebugData_Interval);
-    Request_DebugData = FALSE;
+    request_DebugData = FALSE;
   }
-  if( ((Data3D_Interval && CheckDelay(Data3D_Timer)) || Request_Data3D) && txd_complete) {
+  if( ((Data3D_Interval && CheckDelay(Data3D_Timer)) || request_Data3D) && txd_complete) {
     SendOutData('C', FC_ADDRESS, 1,(uint8_t *) &Data3D, sizeof(Data3D));
     Data3D.AngleNick = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1°
     Data3D.AngleRoll = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1°
     Data3D.Heading   = (int16_t)((10 * yawGyroHeading)   / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1°
     Data3D_Timer = SetDelay(Data3D_Interval);
-    Request_Data3D = FALSE;
+    request_Data3D = FALSE;
   }
-  if(Request_ExternalControl && txd_complete) {
+  if(request_ExternalControl && txd_complete) {
     SendOutData('G', FC_ADDRESS, 1,(uint8_t *) &externalControl, sizeof(externalControl));
-    Request_ExternalControl = FALSE;
+    request_ExternalControl = FALSE;
+  }
 #ifdef USE_MK3MAG
   if((CheckDelay(Compass_Timer)) && txd_complete) {
-    ToMk3Mag.Attitude[0] = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCH_ROLL);  // approx. 0.1 deg
+    ToMk3Mag.Attitude[0] = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL);  // approx. 0.1 deg
-    ToMk3Mag.Attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCH_ROLL);  // approx. 0.1 deg
+    ToMk3Mag.Attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL);  // approx. 0.1 deg
     ToMk3Mag.UserParam[0] = dynamicParams.UserParams[0];
     ToMk3Mag.UserParam[1] = dynamicParams.UserParams[1];
-    ToMk3Mag.CalState = CompassCalState;
+    ToMk3Mag.CalState = compassCalState;
     SendOutData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &ToMk3Mag,sizeof(ToMk3Mag));
     // the last state is 5 and should be send only once to avoid multiple flash writing
-    if(CompassCalState > 4)  CompassCalState = 0;
+    if(compassCalState > 4)  compassCalState = 0;
+    Compass_Timer = SetDelay(99);
+  }
+#endif
+  if(request_MotorTest && txd_complete) {
-    Compass_Timer = SetDelay(99);
+    SendOutData('T', FC_ADDRESS, 0);
     request_MotorTest = FALSE;

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/uart0.c @ 1796 - Rev 1646 → 1775