Rev 1713 | Rev 1762 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1713 | Rev 1760 | ||
---|---|---|---|
Line 1... | Line 1... | ||
1 | #include "main.h" |
1 | #include "main.h" |
2 | - | ||
- | 2 | #define MULTIPLYER 4 |
|
Line 3... | Line 3... | ||
3 | 3 | ||
4 | volatile unsigned int CountMilliseconds = 0; |
4 | volatile unsigned int CountMilliseconds = 0; |
5 | volatile static unsigned int tim_main; |
5 | volatile static unsigned int tim_main; |
6 | volatile unsigned char UpdateMotor = 0; |
6 | volatile unsigned char UpdateMotor = 0; |
7 | volatile unsigned int cntKompass = 0; |
7 | volatile unsigned int cntKompass = 0; |
8 | volatile unsigned int beeptime = 0; |
8 | volatile unsigned int beeptime = 0; |
- | 9 | volatile unsigned char SendSPI = 0, ServoActive = 0, CalculateServoSignals = 1; |
|
- | 10 | uint16_t RemainingPulse = 0; |
|
- | 11 | volatile int16_t ServoNickOffset = (255 / 2) * MULTIPLYER * 16; // initial value near center positon |
|
Line 9... | Line 12... | ||
9 | volatile unsigned char SendSPI = 0, ServoActive = 0; |
12 | volatile int16_t ServoRollOffset = (255 / 2) * MULTIPLYER * 16; // initial value near center positon |
Line 10... | Line 13... | ||
10 | 13 | ||
11 | unsigned int BeepMuster = 0xffff; |
14 | unsigned int BeepMuster = 0xffff; |
Line 183... | Line 186... | ||
183 | 186 | ||
184 | /*****************************************************/ |
187 | /*****************************************************/ |
185 | /* Control Servo Position */ |
188 | /* Control Servo Position */ |
Line -... | Line 189... | ||
- | 189 | /*****************************************************/ |
|
- | 190 | ||
- | 191 | ||
- | 192 | void CalculateServo(void) |
|
- | 193 | { |
|
- | 194 | long cosinus, sinus; |
|
- | 195 | long nick, roll; |
|
- | 196 | ||
- | 197 | cosinus = sintab[EE_Parameter.CamOrientation + 6]; |
|
- | 198 | sinus = sintab[EE_Parameter.CamOrientation]; |
|
- | 199 | ||
- | 200 | if(CalculateServoSignals == 1) |
|
- | 201 | { |
|
- | 202 | nick = ((cosinus * (IntegralNick / (8 * 128L))) - (sinus * (IntegralRoll / (8 * 128L)))); |
|
- | 203 | //nick = (IntegralNick / 128L); |
|
- | 204 | ||
- | 205 | ServoNickOffset += ((int16_t)Parameter_ServoNickControl * (MULTIPLYER*16) - ServoNickOffset) / EE_Parameter.ServoManualControlSpeed; |
|
- | 206 | ServoNickValue = ServoNickOffset / 16; // offset (Range from 0 to 255 * 3 = 765) |
|
- | 207 | if(EE_Parameter.ServoCompInvert & 0x01) |
|
- | 208 | { // inverting movement of servo |
|
- | 209 | ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * nick) / (256L) ); |
|
- | 210 | } |
|
- | 211 | else |
|
- | 212 | { // non inverting movement of servo |
|
- | 213 | ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * nick) / (256L) ); |
|
- | 214 | } |
|
- | 215 | // limit servo value to its parameter range definition |
|
- | 216 | if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) ) |
|
- | 217 | { |
|
- | 218 | ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER; |
|
- | 219 | } |
|
- | 220 | else |
|
- | 221 | if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) ) |
|
- | 222 | { |
|
- | 223 | ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER; |
|
- | 224 | } |
|
- | 225 | if(PlatinenVersion < 20) CalculateServoSignals = 0; else CalculateServoSignals++; |
|
- | 226 | } |
|
- | 227 | else |
|
- | 228 | { |
|
- | 229 | roll = ((cosinus * (IntegralRoll / (8 * 128L))) + (sinus * (IntegralNick / (8 * 128L)))); |
|
- | 230 | ||
- | 231 | ServoRollOffset += ((int16_t)Parameter_ServoRollControl * (MULTIPLYER*16) - ServoRollOffset) / EE_Parameter.ServoManualControlSpeed; |
|
- | 232 | ServoRollValue = ServoRollOffset/16; // offset (Range from 0 to 255 * 3 = 765) |
|
- | 233 | if(EE_Parameter.ServoCompInvert & 0x02) |
|
- | 234 | { // inverting movement of servo |
|
- | 235 | ServoRollValue += (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * roll ) / (256L) ); |
|
- | 236 | } |
|
- | 237 | else |
|
- | 238 | { // non inverting movement of servo |
|
- | 239 | ServoRollValue -= (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * roll ) / (256L) ); |
|
- | 240 | } |
|
- | 241 | // limit servo value to its parameter range definition |
|
- | 242 | if(ServoRollValue < ((int16_t)EE_Parameter.ServoRollMin * MULTIPLYER) ) |
|
- | 243 | { |
|
- | 244 | ServoRollValue = (int16_t)EE_Parameter.ServoRollMin * MULTIPLYER; |
|
- | 245 | } |
|
- | 246 | else |
|
- | 247 | if(ServoRollValue > ((int16_t)EE_Parameter.ServoRollMax * MULTIPLYER) ) |
|
- | 248 | { |
|
- | 249 | ServoRollValue = (int16_t)EE_Parameter.ServoRollMax * MULTIPLYER; |
|
- | 250 | } |
|
- | 251 | CalculateServoSignals = 0; |
|
- | 252 | } |
|
186 | /*****************************************************/ |
253 | } |
187 | 254 | ||
188 | ISR(TIMER2_COMPA_vect) |
255 | ISR(TIMER2_COMPA_vect) |
189 | { |
256 | { |
190 | // frame len 22.5 ms = 14063 * 1.6 us |
257 | // frame len 22.5 ms = 14063 * 1.6 us |
191 | // stop pulse: 0.3 ms = 188 * 1.6 us |
258 | // stop pulse: 0.3 ms = 188 * 1.6 us |
192 | // min servo pulse: 0.6 ms = 375 * 1.6 us |
259 | // min servo pulse: 0.6 ms = 375 * 1.6 us |
Line 193... | Line 260... | ||
193 | // max servo pulse: 2.4 ms = 1500 * 1.6 us |
260 | // max servo pulse: 2.4 ms = 1500 * 1.6 us |
194 | // resolution: 1500 - 375 = 1125 steps |
261 | // resolution: 1500 - 375 = 1125 steps |
195 | - | ||
196 | #define IRS_RUNTIME 127 |
262 | |
197 | #define PPM_STOPPULSE 188 |
263 | #define IRS_RUNTIME 127 |
198 | // #define PPM_FRAMELEN (14063 |
264 | #define PPM_STOPPULSE 188 |
199 | #define PPM_FRAMELEN (1757 * EE_Parameter.ServoNickRefresh) |
265 | #define PPM_FRAMELEN (1757 * EE_Parameter.ServoNickRefresh) |
Line 200... | Line 266... | ||
200 | #define MINSERVOPULSE 375 |
266 | #define MINSERVOPULSE 375 |
201 | #define MAXSERVOPULSE 1500 |
- | |
202 | #define SERVORANGE (MAXSERVOPULSE - MINSERVOPULSE) |
267 | #define MAXSERVOPULSE 1500 |
203 | 268 | #define SERVORANGE (MAXSERVOPULSE - MINSERVOPULSE) |
|
Line 204... | Line -... | ||
204 | static uint8_t PulseOutput = 0; |
- | |
205 | static uint16_t RemainingPulse = 0; |
- | |
206 | static uint16_t ServoFrameTime = 0; |
- | |
Line 207... | Line 269... | ||
207 | static uint8_t ServoIndex = 0; |
269 | |
208 | 270 | static uint8_t PulseOutput = 0; |
|
209 | #define MULTIPLYER 4 |
271 | static uint16_t ServoFrameTime = 0; |
210 | static int16_t ServoNickOffset = (255 / 2) * MULTIPLYER; // initial value near center positon |
272 | static uint8_t ServoIndex = 0; |
Line 219... | Line 281... | ||
219 | { |
281 | { |
220 | if(TCCR2A & (1<<COM2A0)) // we had a low pulse |
282 | if(TCCR2A & (1<<COM2A0)) // we had a low pulse |
221 | { |
283 | { |
222 | TCCR2A &= ~(1<<COM2A0);// make a high pulse |
284 | TCCR2A &= ~(1<<COM2A0);// make a high pulse |
223 | RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms |
285 | RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms |
224 | - | ||
225 | ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset |
- | |
226 | ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765) |
- | |
227 | if(EE_Parameter.ServoCompInvert & 0x01) |
- | |
228 | { // inverting movement of servo |
- | |
229 | ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) ); |
- | |
230 | } |
- | |
231 | else |
- | |
232 | { // non inverting movement of servo |
- | |
233 | ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) ); |
- | |
234 | } |
- | |
235 | // limit servo value to its parameter range definition |
- | |
236 | if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) ) |
- | |
237 | { |
- | |
238 | ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER; |
- | |
239 | } |
- | |
240 | else |
- | |
241 | if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) ) |
- | |
242 | { |
- | |
243 | ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER; |
- | |
244 | } |
- | |
245 | - | ||
246 | RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
286 | RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
247 | - | ||
248 | ServoNickValue /= MULTIPLYER; |
- | |
249 | - | ||
250 | // range servo pulse width |
287 | // range servo pulse width |
251 | if(RemainingPulse > MAXSERVOPULSE ) RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit |
288 | if(RemainingPulse > MAXSERVOPULSE ) RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit |
252 | else if(RemainingPulse < MINSERVOPULSE ) RemainingPulse = MINSERVOPULSE; // lower servo pulse limit |
289 | else if(RemainingPulse < MINSERVOPULSE ) RemainingPulse = MINSERVOPULSE; // lower servo pulse limit |
253 | // accumulate time for correct update rate |
290 | // accumulate time for correct update rate |
254 | ServoFrameTime = RemainingPulse; |
291 | ServoFrameTime = RemainingPulse; |
Line 283... | Line 320... | ||
283 | { |
320 | { |
284 | RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms |
321 | RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms |
285 | switch(ServoIndex) // map servo channels |
322 | switch(ServoIndex) // map servo channels |
286 | { |
323 | { |
287 | case 1: // Nick Compensation Servo |
324 | case 1: // Nick Compensation Servo |
288 | ServoNickOffset = (ServoNickOffset * 3 + (int16_t)Parameter_ServoNickControl * MULTIPLYER) / 4; // lowpass offset |
- | |
289 | ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765) |
- | |
290 | if(EE_Parameter.ServoCompInvert & 0x01) |
- | |
291 | { // inverting movement of servo |
- | |
292 | ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) ); |
- | |
293 | } |
- | |
294 | else |
- | |
295 | { // non inverting movement of servo |
- | |
296 | ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * (IntegralNick / 128L ) ) / (256L) ); |
- | |
297 | } |
- | |
298 | // limit servo value to its parameter range definition |
- | |
299 | if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) ) |
- | |
300 | { |
- | |
301 | ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER; |
- | |
302 | } |
- | |
303 | else |
- | |
304 | if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) ) |
- | |
305 | { |
- | |
306 | ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER; |
- | |
307 | } |
- | |
308 | RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
325 | RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
309 | ServoNickValue /= MULTIPLYER; |
- | |
310 | break; |
326 | break; |
311 | case 2: // Roll Compensation Servo |
327 | case 2: // Roll Compensation Servo |
312 | ServoRollOffset = (ServoRollOffset * 3 + (int16_t) Parameter_ServoRollControl * MULTIPLYER) / 4; // lowpass offset |
- | |
313 | ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765) |
- | |
314 | if(EE_Parameter.ServoCompInvert & 0x02) |
- | |
315 | { // inverting movement of servo |
- | |
316 | ServoRollValue += (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) ); |
- | |
317 | } |
- | |
318 | else |
- | |
319 | { // non inverting movement of servo |
- | |
320 | ServoRollValue -= (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * (IntegralRoll / 128L ) ) / (256L) ); |
- | |
321 | } |
- | |
322 | // limit servo value to its parameter range definition |
- | |
323 | if(ServoRollValue < ((int16_t)EE_Parameter.ServoRollMin * MULTIPLYER) ) |
- | |
324 | { |
- | |
325 | ServoRollValue = (int16_t)EE_Parameter.ServoRollMin * MULTIPLYER; |
- | |
326 | } |
- | |
327 | else |
- | |
328 | if(ServoRollValue > ((int16_t)EE_Parameter.ServoRollMax * MULTIPLYER) ) |
- | |
329 | { |
- | |
330 | ServoRollValue = (int16_t)EE_Parameter.ServoRollMax * MULTIPLYER; |
- | |
331 | } |
- | |
332 | RemainingPulse += ServoRollValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
328 | RemainingPulse += ServoRollValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
333 | ServoRollValue /= MULTIPLYER; |
- | |
334 | //DebugOut.Analog[20] = ServoRollValue; |
- | |
335 | break; |
329 | break; |
336 | case 3: |
330 | case 3: |
337 | RemainingPulse += ((int16_t)Parameter_Servo3 * MULTIPLYER) - (256 / 2) * MULTIPLYER; |
331 | RemainingPulse += ((int16_t)Parameter_Servo3 * MULTIPLYER) - (256 / 2) * MULTIPLYER; |
338 | break; |
332 | break; |
339 | case 4: |
333 | case 4: |
Line 363... | Line 357... | ||
363 | // accumulate time for correct sync gap |
357 | // accumulate time for correct sync gap |
364 | ServoFrameTime += RemainingPulse; |
358 | ServoFrameTime += RemainingPulse; |
365 | if((ServoActive && SenderOkay > 180) || ServoActive == 2) HEF4017R_OFF; // disable HEF4017 reset |
359 | if((ServoActive && SenderOkay > 180) || ServoActive == 2) HEF4017R_OFF; // disable HEF4017 reset |
366 | else HEF4017R_ON; |
360 | else HEF4017R_ON; |
367 | ServoIndex++; // change to next servo channel |
361 | ServoIndex++; // change to next servo channel |
368 | if(ServoIndex > EE_Parameter.ServoNickRefresh) ServoIndex = 0; // reset to the sync gap |
362 | if(ServoIndex > EE_Parameter.ServoNickRefresh) |
- | 363 | { |
|
- | 364 | CalculateServoSignals = 1; |
|
- | 365 | ServoIndex = 0; // reset to the sync gap |
|
- | 366 | } |
|
369 | } |
367 | } |
370 | // set pulse output active |
368 | // set pulse output active |
371 | PulseOutput = 1; |
369 | PulseOutput = 1; |
372 | } |
370 | } |
373 | } // EOF PPM state machine |
371 | } // EOF PPM state machine |