Rev 1775 | Rev 1821 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1775 | Rev 1796 | ||
---|---|---|---|
Line 166... | Line 166... | ||
166 | NewPpmData = 0; // Null means NewData for the first 4 channels |
166 | NewPpmData = 0; // Null means NewData for the first 4 channels |
167 | } |
167 | } |
168 | // synchronize channel index |
168 | // synchronize channel index |
169 | index = 1; |
169 | index = 1; |
170 | } else { // within the PPM frame |
170 | } else { // within the PPM frame |
171 | if(index < MAX_CHANNELS-1) // PPM24 supports 12 channels |
171 | if(index < MAX_CHANNELS-1) { // PPM24 supports 12 channels |
172 | { |
- | |
173 | // check for valid signal length (0.8 ms < signal < 2.1984 ms) |
172 | // check for valid signal length (0.8 ms < signal < 2.1984 ms) |
174 | // signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625 |
173 | // signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625 |
175 | if((signal > 250) && (signal < 687)) { |
174 | if((signal > 250) && (signal < 687)) { |
176 | // shift signal to zero symmetric range -154 to 159 |
175 | // shift signal to zero symmetric range -154 to 159 |
177 | signal -= 470; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms) |
176 | signal -= 470; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms) |
178 | // check for stable signal |
177 | // check for stable signal |
179 | if(abs(signal - PPM_in[index]) < 6) { |
178 | if(abs(signal - PPM_in[index]) < 6) { |
180 | if(RC_Quality < 200) RC_Quality +=10; |
179 | if(RC_Quality < 200) RC_Quality +=10; |
181 | else RC_Quality = 200; |
180 | else RC_Quality = 200; |
182 | } |
181 | } |
183 | // If signal is the same as before +/- 1, just keep it there. |
182 | // If signal is the same as before +/- 1, just keep it there. |
184 | if (signal>=PPM_in[index]-1 && signal<=PPM_in[index]+1) { |
183 | if (signal>=PPM_in[index]-1 && signal<=PPM_in[index]+1) { |
185 | // In addition, if the signal is very close to 0, just set it to 0. |
184 | // In addition, if the signal is very close to 0, just set it to 0. |
186 | if (signal >=-1 && signal <= 1) { |
185 | if (signal >=-1 && signal <= 1) { |
187 | tmp = 0; |
186 | tmp = 0; |
188 | } else { |
187 | } else { |
189 | tmp = PPM_in[index]; |
188 | tmp = PPM_in[index]; |
190 | } |
- | |
191 | } |
- | |
192 | else |
- | |
193 | tmp = signal; |
- | |
194 | // calculate signal difference on good signal level |
- | |
195 | if(RC_Quality >= 195) |
- | |
196 | PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction |
- | |
197 | else PPM_diff[index] = 0; |
- | |
198 | PPM_in[index] = tmp; // update channel value |
- | |
199 | } |
189 | } |
- | 190 | } |
|
- | 191 | else |
|
200 | index++; // next channel |
192 | tmp = signal; |
201 | // demux sum signal for channels 5 to 7 to J3, J4, J5 |
193 | // calculate signal difference on good signal level |
202 | // TODO: General configurability of this R/C channel forwarding. Or remove it completely - the |
- | |
203 | // channels are usually available at the receiver anyway. |
- | |
204 | // if(index == 5) J3HIGH; else J3LOW; |
194 | if(RC_Quality >= 195) |
205 | // if(index == 6) J4HIGH; else J4LOW; |
195 | PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction |
206 | // if(CPUType != ATMEGA644P) // not used as TXD1 |
196 | else PPM_diff[index] = 0; |
207 | // { |
- | |
208 | // if(index == 7) J5HIGH; else J5LOW; |
197 | PPM_in[index] = tmp; // update channel value |
209 | // } |
- | |
210 | } |
198 | } |
- | 199 | index++; // next channel |
|
- | 200 | // demux sum signal for channels 5 to 7 to J3, J4, J5 |
|
- | 201 | // TODO: General configurability of this R/C channel forwarding. Or remove it completely - the |
|
- | 202 | // channels are usually available at the receiver anyway. |
|
- | 203 | // if(index == 5) J3HIGH; else J3LOW; |
|
- | 204 | // if(index == 6) J4HIGH; else J4LOW; |
|
- | 205 | // if(CPUType != ATMEGA644P) // not used as TXD1 |
|
- | 206 | // { |
|
- | 207 | // if(index == 7) J5HIGH; else J5LOW; |
|
- | 208 | // } |
|
- | 209 | } |
|
211 | } |
210 | } |
212 | } |
211 | } |
Line 213... | Line 212... | ||
213 | 212 | ||
214 | #define RCChannel(dimension) PPM_in[staticParams.ChannelAssignment[dimension]] |
213 | #define RCChannel(dimension) PPM_in[staticParams.ChannelAssignment[dimension]] |
Line 244... | Line 243... | ||
244 | void RC_update() { |
243 | void RC_update() { |
245 | int16_t tmp1, tmp2; |
244 | int16_t tmp1, tmp2; |
246 | if(RC_Quality) { |
245 | if(RC_Quality) { |
247 | RC_Quality--; |
246 | RC_Quality--; |
248 | if (NewPpmData-- == 0) { |
247 | if (NewPpmData-- == 0) { |
249 | RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.StickP - /* stickOffsetPitch */ + RCDiff(CH_PITCH) * staticParams.StickD; |
248 | RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.StickP + RCDiff(CH_PITCH) * staticParams.StickD; |
250 | RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.StickP - /* stickOffsetRoll */ + RCDiff(CH_ROLL) * staticParams.StickD; |
249 | RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.StickP + RCDiff(CH_ROLL) * staticParams.StickD; |
251 | RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + PPM_diff[staticParams.ChannelAssignment[CH_THROTTLE]] * dynamicParams.UserParams[3] + 120; |
250 | RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE) * dynamicParams.UserParams[3] + 120; |
252 | if (RC_PRTY[CONTROL_THROTTLE] < 0) RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative. |
251 | if (RC_PRTY[CONTROL_THROTTLE] < 0) RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative. |
253 | tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW); |
252 | tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW); |
254 | // exponential stick sensitivity in yawing rate |
253 | // exponential stick sensitivity in yawing rate |
255 | tmp2 = (int32_t) staticParams.StickYawP * ((int32_t)tmp1 * abs(tmp1)) / 512L; // expo y = ax + bx^2 |
254 | tmp2 = (int32_t) staticParams.StickYawP * ((int32_t)tmp1 * abs(tmp1)) / 512L; // expo y = ax + bx^2 |
256 | tmp2 += (staticParams.StickYawP * tmp1) / 4; |
255 | tmp2 += (staticParams.StickYawP * tmp1) / 4; |