Subversion Repositories FlightCtrl

Rev

Rev 1762 | Rev 1771 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1762 Rev 1763
Line 197... Line 197...
197 
        cosinus = sintab[EE_Parameter.CamOrientation + 6];
 197 
        cosinus = sintab[EE_Parameter.CamOrientation + 6];
198 
        sinus = sintab[EE_Parameter.CamOrientation];
 198 
        sinus = sintab[EE_Parameter.CamOrientation];
Line 199... Line 199...
199 
 
 199 
 
200 
  if(CalculateServoSignals == 1)
 200 
  if(CalculateServoSignals == 1)
201 
   {
 201 
   {
202 
    nick = (cosinus * IntegralNick) / 128L - (sinus * IntegralRoll) / 128L;
 202 
                nick = (cosinus * IntegralNick) / 128L - (sinus * IntegralRoll) / 128L;
203 
        nick = (EE_Parameter.ServoNickComp * nick) / 512L;
 203 
                nick = ((long)EE_Parameter.ServoNickComp * nick) / 512L;
204 
                                                        ServoNickOffset += ((int16_t)Parameter_ServoNickControl * (MULTIPLYER*16) - ServoNickOffset) / EE_Parameter.ServoManualControlSpeed;
 204 
                ServoNickOffset += ((int16_t)Parameter_ServoNickControl * (MULTIPLYER*16) - ServoNickOffset) / EE_Parameter.ServoManualControlSpeed;
205 
                                                        ServoNickValue = ServoNickOffset / 16; // offset (Range from 0 to 255 * 3 = 765)
 205 
                ServoNickValue = ServoNickOffset / 16; // offset (Range from 0 to 255 * 3 = 765)
206 
                                                        if(EE_Parameter.ServoCompInvert & 0x01)
 206 
                if(EE_Parameter.ServoCompInvert & 0x01)
207 
                                                        {       // inverting movement of servo
 207 
                {       // inverting movement of servo
208 
                                                                ServoNickValue += nick;//(int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * nick) / (256L) );
 208 
                        ServoNickValue += nick;//(int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * nick) / (256L) );
209 
                                                        }
 209 
                }
210 
                                                        else
 210 
                else
211 
                                                        {       // non inverting movement of servo
 211 
                {       // non inverting movement of servo
212 
                                                                ServoNickValue -= nick;
 212 
                        ServoNickValue -= nick;
213 
                                                        }
 213 
                }
214 
                                                        // limit servo value to its parameter range definition
 214 
                // limit servo value to its parameter range definition
215 
                                                        if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) )
 215 
                if(ServoNickValue < ((int16_t)EE_Parameter.ServoNickMin * MULTIPLYER) )
216 
                                                        {
 216 
                {
217 
                                                                ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER;
 217 
                        ServoNickValue = (int16_t)EE_Parameter.ServoNickMin * MULTIPLYER;
218 
                                                        }
 218 
                }
219 
                                                        else
 219 
                else
220 
                                                        if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) )
 220 
                if(ServoNickValue > ((int16_t)EE_Parameter.ServoNickMax * MULTIPLYER) )
221 
                                                        {
 221 
                {
222 
                                                                ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER;
 222 
                        ServoNickValue = (int16_t)EE_Parameter.ServoNickMax * MULTIPLYER;
223 
                                                        }
 223 
                }
224 
                                                        if(PlatinenVersion < 20) CalculateServoSignals = 0; else CalculateServoSignals++;
 224 
                if(PlatinenVersion < 20) CalculateServoSignals = 0; else CalculateServoSignals++;
225 
        }
 225 
        }
226 
        else
 226 
        else
227 
        {
 227 
        {
228 
    roll = (cosinus * IntegralRoll) / 128L + (sinus * IntegralNick) / 128L;
 228 
        roll = (cosinus * IntegralRoll) / 128L + (sinus * IntegralNick) / 128L;
229 
    roll = (EE_Parameter.ServoRollComp * roll) / 512L;
 229 
        roll = ((long)EE_Parameter.ServoRollComp * roll) / 512L;
230 
                                                        ServoRollOffset += ((int16_t)Parameter_ServoRollControl * (MULTIPLYER*16) - ServoRollOffset) / EE_Parameter.ServoManualControlSpeed;
 230 
                ServoRollOffset += ((int16_t)Parameter_ServoRollControl * (MULTIPLYER*16) - ServoRollOffset) / EE_Parameter.ServoManualControlSpeed;
231 
                                                        ServoRollValue = ServoRollOffset/16; // offset (Range from 0 to 255 * 3 = 765)
 231 
                ServoRollValue = ServoRollOffset/16; // offset (Range from 0 to 255 * 3 = 765)
232 
                                                        if(EE_Parameter.ServoCompInvert & 0x02)
 232 
                if(EE_Parameter.ServoCompInvert & 0x02)
233 
                                                        {       // inverting movement of servo
 233 
                {       // inverting movement of servo
234 
                                                                ServoRollValue += roll;
 234 
                        ServoRollValue += roll;
235 
                                                        }
 235 
                }
236 
                                                        else
 236 
                else
237 
                                                        {       // non inverting movement of servo
 237 
                {       // non inverting movement of servo
238 
                                                                ServoRollValue -= roll;
 238 
                        ServoRollValue -= roll;
239 
                                                        }
 239 
                }
240 
                                                    // limit servo value to its parameter range definition
 240 
                // limit servo value to its parameter range definition
241 
                                                        if(ServoRollValue < ((int16_t)EE_Parameter.ServoRollMin * MULTIPLYER) )
 241 
                if(ServoRollValue < ((int16_t)EE_Parameter.ServoRollMin * MULTIPLYER) )
242 
                                                        {
 242 
                {
243 
                                                                ServoRollValue = (int16_t)EE_Parameter.ServoRollMin * MULTIPLYER;
 243 
                        ServoRollValue = (int16_t)EE_Parameter.ServoRollMin * MULTIPLYER;
244 
                                                        }
 244 
                }
245 
                                                        else
 245 
                else
246 
                                                        if(ServoRollValue > ((int16_t)EE_Parameter.ServoRollMax * MULTIPLYER) )
 246 
                if(ServoRollValue > ((int16_t)EE_Parameter.ServoRollMax * MULTIPLYER) )
247 
                                                        {
 247 
                {
248 
                                                                ServoRollValue = (int16_t)EE_Parameter.ServoRollMax * MULTIPLYER;
 248 
                        ServoRollValue = (int16_t)EE_Parameter.ServoRollMax * MULTIPLYER;
249 
                                                        }
 249 
                }
250 
                                                        CalculateServoSignals = 0;
 250 
                CalculateServoSignals = 0;
251 
        }
 251 
        }
Line 252... Line 252...
252 
}
 252 
}
253 
 
 253 
 
Line 291... Line 291...
291 
                        }
 291 
                        }
292 
                        else // we had a high pulse
 292 
                        else // we had a high pulse
293 
                        {
 293 
                        {
294 
                                TCCR2A |= (1<<COM2A0); // make a low pulse
 294 
                                TCCR2A |= (1<<COM2A0); // make a low pulse
295 
                                RemainingPulse = PPM_FRAMELEN - ServoFrameTime;
 295 
                                RemainingPulse = PPM_FRAMELEN - ServoFrameTime;
- 
 
 296 
                                CalculateServoSignals = 1;
296 
                        }
 297 
                        }
297 
                        // set pulse output active
 298 
                        // set pulse output active
298 
                        PulseOutput = 1;
 299 
                        PulseOutput = 1;
299 
                }
 300 
                }
300 
        } // EOF Nick servo state machine
 301 
        } // EOF Nick servo state machine
Line 356... Line 357...
356 
                                // accumulate time for correct sync gap
 357 
                                // accumulate time for correct sync gap
357 
                                ServoFrameTime += RemainingPulse;
 358 
                                ServoFrameTime += RemainingPulse;
358 
                                if((ServoActive && SenderOkay > 180) || ServoActive == 2) HEF4017R_OFF; // disable HEF4017 reset
 359 
                                if((ServoActive && SenderOkay > 180) || ServoActive == 2) HEF4017R_OFF; // disable HEF4017 reset
359 
                                else HEF4017R_ON;
 360 
                                else HEF4017R_ON;
360 
                                ServoIndex++; // change to next servo channel
 361 
                                ServoIndex++; // change to next servo channel
361 
                                if(ServoIndex > EE_Parameter.ServoNickRefresh)
 362 
                                if(ServoIndex > EE_Parameter.ServoNickRefresh)
362 
                                  {
 363 
                                  {
363 
                                    CalculateServoSignals = 1;
 364 
                                    CalculateServoSignals = 1;
364 
                                        ServoIndex = 0; // reset to the sync gap
 365 
                                        ServoIndex = 0; // reset to the sync gap
365 
                                  }
 366 
                                  }
366 
                        }
 367 
                        }
367 
                        // set pulse output active
 368 
                        // set pulse output active
368 
                        PulseOutput = 1;
 369 
                        PulseOutput = 1;