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

 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // This code has been derived from the implementation of Stefan Engelke.
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 /*
-    Copyright (c) 2008 Stefan Engelke <stefan@tinkerer.eu>
+ Copyright (c) 2008 Stefan Engelke <stefan@tinkerer.eu>
-    Permission is hereby granted, free of charge, to any person
+ Permission is hereby granted, free of charge, to any person
-    obtaining a copy of this software and associated documentation
+ obtaining a copy of this software and associated documentation
-    files (the "Software"), to deal in the Software without
+ files (the "Software"), to deal in the Software without
-    restriction, including without limitation the rights to use, copy,
+ restriction, including without limitation the rights to use, copy,
-    modify, merge, publish, distribute, sublicense, and/or sell copies
+ modify, merge, publish, distribute, sublicense, and/or sell copies
-    of the Software, and to permit persons to whom the Software is
+ of the Software, and to permit persons to whom the Software is
-    furnished to do so, subject to the following conditions:
+ furnished to do so, subject to the following conditions:
-    The above copyright notice and this permission notice shall be
+ The above copyright notice and this permission notice shall be
-    included in all copies or substantial portions of the Software.
+ included in all copies or substantial portions of the Software.
-    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
-    EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
-    MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
-    NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+ NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
-    HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+ HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
-    WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
-    DEALINGS IN THE SOFTWARE.
+ DEALINGS IN THE SOFTWARE.
-    $Id: rcdsl.c 60 2008-08-21 07:50:48Z taser $
+ $Id: rcdsl.c 60 2008-08-21 07:50:48Z taser $
-    RCDSL.H and RCDSL.C is an INOFFICIAL implementation of the
+ RCDSL.H and RCDSL.C is an INOFFICIAL implementation of the
-    communication protocol used by DSL receivers of Act Europe.
+ communication protocol used by DSL receivers of Act Europe.
-    The DSL receivers have a serial communication port to connect
+ The DSL receivers have a serial communication port to connect
-    two receivers in diversity mode. Each receiver is sending the
+ two receivers in diversity mode. Each receiver is sending the
-    received servo signals periodically over this port. This fact
+ received servo signals periodically over this port. This fact
-    can be used to connect the receiver to the control unit of the
+ can be used to connect the receiver to the control unit of the
-    model via UART instead of evaluating the PPM signal.
+ model via UART instead of evaluating the PPM signal.
-    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.
-*/
+ */
 /*
-Connection of DSL to SV1 of FC:
+ Connection of DSL to SV1 of FC:
-( DSL Pin1 is on side of channel 4 )
+ ( DSL Pin1 is on side of channel 4 )
 . GND <--> pin 7 (GND)
 . TXD <--> pin 3 (RXD1 Atmega644p)
 . RXD <--> pin 4 (TXD1 Atmega644p) optional
 . 5V  <--> pin 2 (5V)
-Do not connect the receiver via PPM-Sumsignal output the same time.
+ Do not connect the receiver via PPM-Sumsignal output the same time.
-Data are send at every 20 ms @ 38400 Baud 8-N-1
+ Data are send at every 20 ms @ 38400 Baud 8-N-1
-Data Frame: |0xFF|0xFF|0x1F|FREQALLOC|??|RSSI|VBAT|??|CRC|10|CH0D1|CH0D0|CH1D1|CH1D0|CRC| ...etc
+ Data Frame: |0xFF|0xFF|0x1F|FREQALLOC|??|RSSI|VBAT|??|CRC|10|CH0D1|CH0D0|CH1D1|CH1D0|CRC| ...etc
-FREQALLOC = 35, 40, 72
+ FREQALLOC = 35, 40, 72
-RSSI = 0.. 255 // Received signal strength indicator
+ RSSI = 0.. 255 // Received signal strength indicator
-VBAT = 0...255 // supply voltage (0.0V.. 7.8V)
+ VBAT = 0...255 // supply voltage (0.0V.. 7.8V)
-Servo Pair:   |0x1X|CHXD1|CHXD0|CHX+1D1|CHX+1D0|CRC|
+ Servo Pair:   |0x1X|CHXD1|CHXD0|CHX+1D1|CHX+1D0|CRC|
-X is channel index of 1 servo value
+ X is channel index of 1 servo value
-D1D0 is servo value as u16 in range of 7373 (1ms) to 14745 (2ms)
+ D1D0 is servo value as u16 in range of 7373 (1ms) to 14745 (2ms)
-there are 8 channels submitted, i.e 4 servo pairs
+ there are 8 channels submitted, i.e 4 servo pairs
-Frame examples with signel received
+ Frame examples with signel received
-FFFF 1F23F079A304AD 1036012B1E6F 122AFB2AECB2 142B4D2B4404 1636872B33CE
+ FFFF 1F23F079A304AD 1036012B1E6F 122AFB2AECB2 142B4D2B4404 1636872B33CE
-FFFF 1F23F079A304AD 1036002B1F6F 122AFE2AEBB0 142B4B2B4406 1636872B33CE
+ FFFF 1F23F079A304AD 1036002B1F6F 122AFE2AEBB0 142B4B2B4406 1636872B33CE
-FFFF 1F23F079A304AD 1035FF2B226E 122AFC2AEAB3 142B4E2B4304 1636882B33CD
+ FFFF 1F23F079A304AD 1035FF2B226E 122AFC2AEAB3 142B4E2B4304 1636882B33CD
-FFFF 1F23F079A304AD 1036022B1E6E 122AFB2AEEB0 142B4A2B4506 1636872B33CE
+ FFFF 1F23F079A304AD 1036022B1E6E 122AFB2AEEB0 142B4A2B4506 1636872B33CE
-FFFF 1F23F079A304AD 1036022B1E6E 122AFE2AEBB0 142B4B2B4406 1636882B33CD
+ FFFF 1F23F079A304AD 1036022B1E6E 122AFE2AEBB0 142B4B2B4406 1636882B33CD
-FFFF 1F23F079A304AD 1036012B1E6F 122AFD2AEAB2 142B4E2B4403 1636862B33CF
+ FFFF 1F23F079A304AD 1036012B1E6F 122AFD2AEAB2 142B4E2B4403 1636862B33CF
-FFFF 1F23F079A304AD 1036032B1D6E 122AFD2AEBB1 142B4C2B4504 1636862B33CF
+ FFFF 1F23F079A304AD 1036032B1D6E 122AFD2AEBB1 142B4C2B4504 1636862B33CF
-Frame examples with no signal received
+ Frame examples with no signal received
-FFFF 1F23F000A30426
+ FFFF 1F23F000A30426
-FFFF 1F23F000A30426
+ FFFF 1F23F000A30426
-FFFF 1F23F000A30426
+ FFFF 1F23F000A30426
-FFFF 1F23F000A30426
+ FFFF 1F23F000A30426
-FFFF 1F23F000A30426
+ FFFF 1F23F000A30426
-FFFF 1F23F000A30426
+ FFFF 1F23F000A30426
-FFFF 1F23F000A30426
+ FFFF 1F23F000A30426
-*/
+ */
 #include <stdlib.h>
 #include "dsl.h"
 #include "rc.h"
 #include "uart0.h"
 uint8_t dsl_RSSI = 0;
 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;
 // 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)
-void dsl_new_signal(uint8_t channel, int16_t signal)
+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;
-                if(RC_Quality < 200) RC_Quality +=10;
+                else
-                else RC_Quality = 200;
+                        RC_Quality = 200;
         }
         // calculate exponential history for signal
+        tmp = (3 * (PPM_in[index]) + signal) / 4;
-        tmp = (3 * (PPM_in[index]) + signal) / 4;
+        if (tmp > signal + 1)
+                tmp--;
-        if(tmp > signal+1) tmp--; else
+        else if (tmp < signal - 1)
+                tmp++;
-        if(tmp < signal-1) tmp++;
+        // calculate signal difference on good signal level
+        if (RC_Quality >= 195)
-        // calculate signal difference on good signal level
+                PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for noise reduction
-        if(RC_Quality >= 195)  PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for noise reduction
+        else
-        else PPM_diff[index] = 0;
+                PPM_diff[index] = 0;
-        PPM_in[index] = tmp; // update channel value
         PPM_in[index] = tmp; // update channel value
     if(index == 4)
         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)
+void dsl_decode_packet(void) {
-{
-    uint8_t  i;
+        uint8_t i;
         // check for header condition
-        if((PacketBuffer[0] & 0xF0) == 0x10)
-        {
+        if ((PacketBuffer[0] & 0xF0) == 0x10) {
-                if(PacketBuffer[0] == 0x1F) // separate status frame
+                if (PacketBuffer[0] == 0x1F) // separate status frame
                 {
-                        dsl_Allocation  = PacketBuffer[1]; // Get frequency allocation
+                        dsl_Allocation = PacketBuffer[1]; // Get frequency allocation
                         // ??                   = PacketBuffer[2];
-                        dsl_RSSI                = PacketBuffer[3]; // Get signal quality
+                        dsl_RSSI = PacketBuffer[3]; // Get signal quality
-                        dsl_Battery     = PacketBuffer[4]; // Get voltage of battery supply
+                        dsl_Battery = PacketBuffer[4]; // Get voltage of battery supply
                         // ??                   = PacketBuffer[5];
-                        if(dsl_RSSI == 0)
                         if (dsl_RSSI == 0) {
                                 RC_Quality = 0;
-                                for (i = 0; i<5; i++)
                                 for (i = 0; i < 5; i++) {
                                         PPM_diff[i] = 0;
                                         PPM_in[i] = 0;
+                                }
+                        }
-                }
-                else // probably a channel pair
+                } else // probably a channel pair
+                {
-                        i = PacketBuffer[0] & 0x0F;   // last 4 bits of the header indicates the channel pair
+                        i = PacketBuffer[0] & 0x0F; // last 4 bits of the header indicates the channel pair
-                        if(i < 10)// maximum 12 channels
+                        if (i < 10)// maximum 12 channels
+                        {
                                 // big to little endian
                                 ChannelPair.byte[1] = PacketBuffer[1];
                                 ChannelPair.byte[0] = PacketBuffer[2];
                                 ChannelPair.byte[3] = PacketBuffer[3];
                                 ChannelPair.byte[2] = PacketBuffer[4];
-                                dsl_new_signal(i,  ChannelPair.Servo[0]);
+                                dsl_new_signal(i, ChannelPair.Servo[0]);
-                                dsl_new_signal(i+1,ChannelPair.Servo[1]);
+                                dsl_new_signal(i + 1, ChannelPair.Servo[1]);
+                        }
+                }
         } // 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))
+        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
                 crc = ~crc;
-                if (crc == 0xFF) crc = 0xFE;
+                if (crc == 0xFF)
+                        crc = 0xFE;
                 // if crc matches decode the packet
+                if (c == crc)
-        if (c == crc) dsl_decode_packet();
+                        dsl_decode_packet();
-        // handle next packet
+                // handle next packet
-        cnt = 0;
+                cnt = 0;
-        crc = 0;
+                crc = 0;
-        }
-        else // collect channel data bytes
+        } else // collect channel data bytes
+        {
                 PacketBuffer[cnt++] = c;
                 crc += c;
+        }
         // store last byte for sync check
         last_c = c;
+}

Subversion Repositories FlightCtrl

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