WebSVN - FlightCtrl - Diff - Rev 1612 and 1821 - /branches/dongfang_FC

-    If you have any questions, fell free to send me an e-mail.
+ If you have any questions, fell free to send me an e-mail.
 */
  */
 /*
 uint8_t dsl_Battery = 0;
 uint8_t dsl_Allocation = 0;
 uint8_t PacketBuffer[6];
 //uint8_t Jitter = 0; // same measurement as RC_Quality in rc.c
-typedef union
 typedef union {
         int16_t Servo[2];
-        uint8_t  byte[4];
+        uint8_t byte[4];
 } ChannelPair_t;
 ChannelPair_t ChannelPair;
 ChannelPair_t ChannelPair;
 // This function is called, when a new servo signal is properly received.
-// Parameters: servo  - servo number (0-9)
-//             signal - servo signal between 7373 (1ms) and 14745 (2ms)
+// Parameters: servo  - servo number (0-9)
-void dsl_new_signal(uint8_t channel, int16_t signal)
+//             signal - servo signal between 7373 (1ms) and 14745 (2ms)
 void dsl_new_signal(uint8_t channel, int16_t signal) {
-    int16_t tmp;
+        int16_t tmp;
-    uint8_t index = channel + 1; // mk channels start with 1
+        uint8_t index = channel + 1; // mk channels start with 1
         //RC_Quality    = (212 * (uint16_t)dsl_RSSI) / 128; // have to be scaled approx. by a factor of 1.66 to get 200 at full level
         //if(RC_Quality > 255) RC_Quality = 255;
-    // signal from  DSL-receiver is between 7373 (1ms) und 14745 (2ms).
+        // signal from  DSL-receiver is between 7373 (1ms) und 14745 (2ms).
+        signal -= 11059; // shift to neutral
-    signal-= 11059;     // shift to neutral
+        signal /= 24; // scale to mk rc resolution
     signal/= 24;        // scale to mk rc resolution
         if (abs(signal-PPM_in[index]) < 6) {
-        if(abs(signal-PPM_in[index]) < 6)
+                if (RC_Quality < 200)
-        {
+                        RC_Quality += 10;
+                else
-                if(RC_Quality < 200) RC_Quality +=10;
+                        RC_Quality = 200;
+        }
                 else RC_Quality = 200;
+        // calculate exponential history for signal
-        }
+        tmp = (3 * (PPM_in[index]) + signal) / 4;
+        if (tmp > signal + 1)
+                tmp--;
-        // calculate exponential history for signal
+        else if (tmp < signal - 1)
-        tmp = (3 * (PPM_in[index]) + signal) / 4;
+                tmp++;
-        if(tmp > signal+1) tmp--; else
-        if(tmp < signal-1) tmp++;
+        // calculate signal difference on good signal level
-        // calculate signal difference on good signal level
+        if (RC_Quality >= 195)
-        if(RC_Quality >= 195)  PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for noise reduction
+                PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for noise reduction
-        else PPM_diff[index] = 0;
+        else
-        PPM_in[index] = tmp; // update channel value
+                PPM_diff[index] = 0;
+        PPM_in[index] = tmp; // update channel value
-    if(index == 4)
-                NewPpmData = 0;
+        if (index == 4) {
-        }
+                NewPpmData = 0;
-}
+        }
 // This function is called within dsl_parser(), when a complete
 // data packet with valid checksum has been received.
-void dsl_decode_packet(void)
+// This function is called within dsl_parser(), when a complete
 // data packet with valid checksum has been received.
-    uint8_t  i;
+void dsl_decode_packet(void) {
         uint8_t i;
-        // check for header condition
-        if((PacketBuffer[0] & 0xF0) == 0x10)
         // check for header condition
-                if(PacketBuffer[0] == 0x1F) // separate status frame
+        if ((PacketBuffer[0] & 0xF0) == 0x10) {
-                {
-                        dsl_Allocation  = PacketBuffer[1]; // Get frequency allocation
+                if (PacketBuffer[0] == 0x1F) // separate status frame
                         // ??                   = PacketBuffer[2];
-                        dsl_RSSI                = PacketBuffer[3]; // Get signal quality
+                        dsl_Allocation = PacketBuffer[1]; // Get frequency allocation
-                        dsl_Battery     = PacketBuffer[4]; // Get voltage of battery supply
+                        // ??                   = PacketBuffer[2];
-                        // ??                   = PacketBuffer[5];
-                        if(dsl_RSSI == 0)
+                        dsl_RSSI = PacketBuffer[3]; // Get signal quality
                         dsl_Battery = PacketBuffer[4]; // Get voltage of battery supply
-                                RC_Quality = 0;
+                        // ??                   = PacketBuffer[5];
-                                for (i = 0; i<5; i++)
+                        if (dsl_RSSI == 0) {
                                 RC_Quality = 0;
-                                        PPM_diff[i] = 0;
+                                for (i = 0; i < 5; i++) {
+                        }
+                }
         } // EOF header condition
+}
 // this function should be called within the UART RX ISR
-void dsl_parser(uint8_t c)
 void dsl_parser(uint8_t c) {
         static uint8_t last_c = 0;
-        static uint8_t crc      = 0;
+        static uint8_t crc = 0;
         static uint8_t cnt = 0;
         static uint8_t packet_len = 0;
-        // check for sync condition
-        if ((c==0xFF) && (last_c==0xFF))
+        // check for sync condition
         if ((c == 0xFF) && (last_c == 0xFF)) {
                 cnt = 0; // reset byte counter
                 crc = 0; // reset checksum
                 return;
+        }
+        if (cnt == 0) // begin of a packet
+        {
-        if(cnt == 0) // begin of a packet
+                if (c == 0x1F)
                         packet_len = 5; // a status packet has 5 bytes + crc
-                if(c == 0x1F)   packet_len = 5; // a status packet has 5 bytes + crc
+                else
-                else                    packet_len = 4; // a channel pair packet has 4 bytes + crc
+                        packet_len = 4; // a channel pair packet has 4 bytes + crc
+        }
-        if(cnt > packet_len) // packet complete, crc byte received
+        if (cnt > packet_len) // packet complete, crc byte received
         {
+                // calculate checksum
-                // calculate checksum
+                crc = ~crc;
+                if (crc == 0xFF)
-                crc = ~crc;
+                        crc = 0xFE;
-                if (crc == 0xFF) crc = 0xFE;
+                // if crc matches decode the packet
-                // if crc matches decode the packet
+                if (c == crc)
-        if (c == crc) dsl_decode_packet();
+                        dsl_decode_packet();
-        // handle next packet
-        cnt = 0;
+                // handle next packet
-        crc = 0;
+                cnt = 0;
                 crc = 0;
-        else // collect channel data bytes
+        } else // collect channel data bytes
+        {
                 PacketBuffer[cnt++] = c;

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/dsl.c @ 1843 - Rev 1612 → 1821