Rev 1612 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1612 | Rev 1821 | ||
---|---|---|---|
Line 36... | Line 36... | ||
36 | 36 | ||
Line 37... | Line 37... | ||
37 | If you have any questions, fell free to send me an e-mail. |
37 | If you have any questions, fell free to send me an e-mail. |
Line 38... | Line -... | ||
38 | - | ||
39 | */ |
38 | |
40 | 39 | */ |
|
41 | 40 | ||
Line 42... | Line 41... | ||
42 | /* |
41 | /* |
Line 94... | Line 93... | ||
94 | uint8_t dsl_Battery = 0; |
93 | uint8_t dsl_Battery = 0; |
95 | uint8_t dsl_Allocation = 0; |
94 | uint8_t dsl_Allocation = 0; |
96 | uint8_t PacketBuffer[6]; |
95 | uint8_t PacketBuffer[6]; |
97 | //uint8_t Jitter = 0; // same measurement as RC_Quality in rc.c |
96 | //uint8_t Jitter = 0; // same measurement as RC_Quality in rc.c |
Line 98... | Line 97... | ||
98 | 97 | ||
99 | typedef union |
- | |
100 | { |
98 | typedef union { |
101 | int16_t Servo[2]; |
99 | int16_t Servo[2]; |
102 | uint8_t byte[4]; |
100 | uint8_t byte[4]; |
Line 103... | Line 101... | ||
103 | } ChannelPair_t; |
101 | } ChannelPair_t; |
Line 104... | Line -... | ||
104 | - | ||
105 | ChannelPair_t ChannelPair; |
102 | |
106 | 103 | ChannelPair_t ChannelPair; |
|
107 | 104 | ||
108 | // This function is called, when a new servo signal is properly received. |
105 | // This function is called, when a new servo signal is properly received. |
109 | // Parameters: servo - servo number (0-9) |
- | |
110 | // signal - servo signal between 7373 (1ms) and 14745 (2ms) |
106 | // Parameters: servo - servo number (0-9) |
111 | void dsl_new_signal(uint8_t channel, int16_t signal) |
107 | // signal - servo signal between 7373 (1ms) and 14745 (2ms) |
Line 112... | Line 108... | ||
112 | { |
108 | void dsl_new_signal(uint8_t channel, int16_t signal) { |
113 | int16_t tmp; |
109 | int16_t tmp; |
Line 114... | Line 110... | ||
114 | uint8_t index = channel + 1; // mk channels start with 1 |
110 | uint8_t index = channel + 1; // mk channels start with 1 |
115 | 111 | ||
116 | //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 |
112 | //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 |
Line 117... | Line 113... | ||
117 | //if(RC_Quality > 255) RC_Quality = 255; |
113 | //if(RC_Quality > 255) RC_Quality = 255; |
118 | - | ||
119 | // signal from DSL-receiver is between 7373 (1ms) und 14745 (2ms). |
114 | |
- | 115 | // signal from DSL-receiver is between 7373 (1ms) und 14745 (2ms). |
|
- | 116 | signal -= 11059; // shift to neutral |
|
120 | signal-= 11059; // shift to neutral |
117 | signal /= 24; // scale to mk rc resolution |
121 | signal/= 24; // scale to mk rc resolution |
118 | |
Line 122... | Line 119... | ||
122 | 119 | if (abs(signal-PPM_in[index]) < 6) { |
|
123 | if(abs(signal-PPM_in[index]) < 6) |
120 | if (RC_Quality < 200) |
124 | { |
121 | RC_Quality += 10; |
- | 122 | else |
|
125 | if(RC_Quality < 200) RC_Quality +=10; |
123 | RC_Quality = 200; |
- | 124 | } |
|
126 | else RC_Quality = 200; |
125 | |
- | 126 | // calculate exponential history for signal |
|
127 | } |
127 | tmp = (3 * (PPM_in[index]) + signal) / 4; |
- | 128 | if (tmp > signal + 1) |
|
128 | 129 | tmp--; |
|
129 | // calculate exponential history for signal |
130 | else if (tmp < signal - 1) |
Line 130... | Line 131... | ||
130 | tmp = (3 * (PPM_in[index]) + signal) / 4; |
131 | tmp++; |
131 | if(tmp > signal+1) tmp--; else |
- | |
132 | if(tmp < signal-1) tmp++; |
132 | // calculate signal difference on good signal level |
133 | // calculate signal difference on good signal level |
133 | if (RC_Quality >= 195) |
134 | if(RC_Quality >= 195) PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for noise reduction |
134 | PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for noise reduction |
Line 135... | Line 135... | ||
135 | else PPM_diff[index] = 0; |
135 | else |
136 | PPM_in[index] = tmp; // update channel value |
136 | PPM_diff[index] = 0; |
137 | 137 | PPM_in[index] = tmp; // update channel value |
|
138 | if(index == 4) |
- | |
139 | { |
138 | |
Line 140... | Line 139... | ||
140 | NewPpmData = 0; |
139 | if (index == 4) { |
141 | } |
140 | NewPpmData = 0; |
142 | } |
- | |
143 | 141 | } |
|
144 | // This function is called within dsl_parser(), when a complete |
142 | } |
145 | // data packet with valid checksum has been received. |
143 | |
146 | void dsl_decode_packet(void) |
144 | // This function is called within dsl_parser(), when a complete |
147 | { |
145 | // data packet with valid checksum has been received. |
148 | uint8_t i; |
146 | void dsl_decode_packet(void) { |
149 | 147 | uint8_t i; |
|
150 | // check for header condition |
148 | |
151 | if((PacketBuffer[0] & 0xF0) == 0x10) |
- | |
152 | { |
149 | // check for header condition |
153 | if(PacketBuffer[0] == 0x1F) // separate status frame |
150 | if ((PacketBuffer[0] & 0xF0) == 0x10) { |
154 | { |
- | |
155 | dsl_Allocation = PacketBuffer[1]; // Get frequency allocation |
151 | if (PacketBuffer[0] == 0x1F) // separate status frame |
156 | // ?? = PacketBuffer[2]; |
152 | { |
157 | dsl_RSSI = PacketBuffer[3]; // Get signal quality |
153 | dsl_Allocation = PacketBuffer[1]; // Get frequency allocation |
158 | dsl_Battery = PacketBuffer[4]; // Get voltage of battery supply |
154 | // ?? = PacketBuffer[2]; |
159 | // ?? = PacketBuffer[5]; |
- | |
160 | if(dsl_RSSI == 0) |
155 | dsl_RSSI = PacketBuffer[3]; // Get signal quality |
161 | { |
156 | dsl_Battery = PacketBuffer[4]; // Get voltage of battery supply |
162 | RC_Quality = 0; |
157 | // ?? = PacketBuffer[5]; |
163 | for (i = 0; i<5; i++) |
158 | if (dsl_RSSI == 0) { |
164 | { |
159 | RC_Quality = 0; |
165 | PPM_diff[i] = 0; |
160 | for (i = 0; i < 5; i++) { |
Line 182... | Line 177... | ||
182 | } |
177 | } |
183 | } |
178 | } |
184 | } // EOF header condition |
179 | } // EOF header condition |
185 | } |
180 | } |
Line 186... | Line -... | ||
186 | - | ||
187 | 181 | ||
188 | // this function should be called within the UART RX ISR |
182 | // this function should be called within the UART RX ISR |
189 | void dsl_parser(uint8_t c) |
- | |
190 | { |
183 | void dsl_parser(uint8_t c) { |
191 | static uint8_t last_c = 0; |
184 | static uint8_t last_c = 0; |
192 | static uint8_t crc = 0; |
185 | static uint8_t crc = 0; |
193 | static uint8_t cnt = 0; |
186 | static uint8_t cnt = 0; |
Line 194... | Line 187... | ||
194 | static uint8_t packet_len = 0; |
187 | static uint8_t packet_len = 0; |
195 | 188 | ||
196 | // check for sync condition |
- | |
197 | if ((c==0xFF) && (last_c==0xFF)) |
189 | // check for sync condition |
198 | { |
190 | if ((c == 0xFF) && (last_c == 0xFF)) { |
199 | cnt = 0; // reset byte counter |
191 | cnt = 0; // reset byte counter |
200 | crc = 0; // reset checksum |
192 | crc = 0; // reset checksum |
Line 201... | Line 193... | ||
201 | return; |
193 | return; |
202 | } |
194 | } |
- | 195 | ||
203 | 196 | if (cnt == 0) // begin of a packet |
|
- | 197 | { |
|
204 | if(cnt == 0) // begin of a packet |
198 | if (c == 0x1F) |
205 | { |
199 | packet_len = 5; // a status packet has 5 bytes + crc |
206 | if(c == 0x1F) packet_len = 5; // a status packet has 5 bytes + crc |
200 | else |
207 | else packet_len = 4; // a channel pair packet has 4 bytes + crc |
201 | packet_len = 4; // a channel pair packet has 4 bytes + crc |
208 | } |
202 | } |
209 | if(cnt > packet_len) // packet complete, crc byte received |
203 | if (cnt > packet_len) // packet complete, crc byte received |
210 | { |
204 | { |
- | 205 | // calculate checksum |
|
211 | // calculate checksum |
206 | crc = ~crc; |
- | 207 | if (crc == 0xFF) |
|
212 | crc = ~crc; |
208 | crc = 0xFE; |
213 | if (crc == 0xFF) crc = 0xFE; |
209 | // if crc matches decode the packet |
214 | // if crc matches decode the packet |
210 | if (c == crc) |
215 | if (c == crc) dsl_decode_packet(); |
211 | dsl_decode_packet(); |
216 | // handle next packet |
- | |
217 | cnt = 0; |
212 | // handle next packet |
218 | crc = 0; |
213 | cnt = 0; |
219 | } |
214 | crc = 0; |
220 | else // collect channel data bytes |
215 | } else // collect channel data bytes |
221 | { |
216 | { |
222 | PacketBuffer[cnt++] = c; |
217 | PacketBuffer[cnt++] = c; |