Rev 1760 | Rev 1763 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1760 | Rev 1762 | ||
|---|---|---|---|
| Line 189... | Line 189... | ||
| 189 | /*****************************************************/ |
189 | /*****************************************************/ |
| Line 190... | Line 190... | ||
| 190 | 190 | ||
| 191 | 191 | ||
| 192 | void CalculateServo(void) |
192 | void CalculateServo(void) |
| 193 | { |
193 | { |
| Line 194... | Line 194... | ||
| 194 | long cosinus, sinus; |
194 | char cosinus, sinus; |
| 195 | long nick, roll; |
195 | long nick, roll; |
| Line 196... | Line 196... | ||
| 196 | 196 | ||
| 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]; |
| 199 | 199 | ||
| 200 | if(CalculateServoSignals == 1) |
- | |
| 201 | { |
200 | if(CalculateServoSignals == 1) |
| 202 | nick = ((cosinus * (IntegralNick / (8 * 128L))) - (sinus * (IntegralRoll / (8 * 128L)))); |
201 | { |
| 203 | //nick = (IntegralNick / 128L); |
202 | nick = (cosinus * IntegralNick) / 128L - (sinus * IntegralRoll) / 128L; |
| 204 | 203 | nick = (EE_Parameter.ServoNickComp * nick) / 512L; |
|
| 205 | ServoNickOffset += ((int16_t)Parameter_ServoNickControl * (MULTIPLYER*16) - ServoNickOffset) / EE_Parameter.ServoManualControlSpeed; |
204 | ServoNickOffset += ((int16_t)Parameter_ServoNickControl * (MULTIPLYER*16) - ServoNickOffset) / EE_Parameter.ServoManualControlSpeed; |
| 206 | ServoNickValue = ServoNickOffset / 16; // offset (Range from 0 to 255 * 3 = 765) |
205 | ServoNickValue = ServoNickOffset / 16; // offset (Range from 0 to 255 * 3 = 765) |
| 207 | if(EE_Parameter.ServoCompInvert & 0x01) |
206 | if(EE_Parameter.ServoCompInvert & 0x01) |
| 208 | { // inverting movement of servo |
207 | { // inverting movement of servo |
| 209 | ServoNickValue += (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * nick) / (256L) ); |
208 | ServoNickValue += nick;//(int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * nick) / (256L) ); |
| 210 | } |
209 | } |
| 211 | else |
210 | else |
| 212 | { // non inverting movement of servo |
211 | { // non inverting movement of servo |
| 213 | ServoNickValue -= (int16_t)( ( (int32_t)EE_Parameter.ServoNickComp * MULTIPLYER * nick) / (256L) ); |
212 | ServoNickValue -= nick; |
| 214 | } |
213 | } |
| Line 224... | Line 223... | ||
| 224 | } |
223 | } |
| 225 | if(PlatinenVersion < 20) CalculateServoSignals = 0; else CalculateServoSignals++; |
224 | if(PlatinenVersion < 20) CalculateServoSignals = 0; else CalculateServoSignals++; |
| 226 | } |
225 | } |
| 227 | else |
226 | else |
| 228 | { |
227 | { |
| 229 | roll = ((cosinus * (IntegralRoll / (8 * 128L))) + (sinus * (IntegralNick / (8 * 128L)))); |
228 | roll = (cosinus * IntegralRoll) / 128L + (sinus * IntegralNick) / 128L; |
| 230 | - | ||
| - | 229 | roll = (EE_Parameter.ServoRollComp * roll) / 512L; |
|
| 231 | ServoRollOffset += ((int16_t)Parameter_ServoRollControl * (MULTIPLYER*16) - ServoRollOffset) / EE_Parameter.ServoManualControlSpeed; |
230 | ServoRollOffset += ((int16_t)Parameter_ServoRollControl * (MULTIPLYER*16) - ServoRollOffset) / EE_Parameter.ServoManualControlSpeed; |
| 232 | ServoRollValue = ServoRollOffset/16; // offset (Range from 0 to 255 * 3 = 765) |
231 | ServoRollValue = ServoRollOffset/16; // offset (Range from 0 to 255 * 3 = 765) |
| 233 | if(EE_Parameter.ServoCompInvert & 0x02) |
232 | if(EE_Parameter.ServoCompInvert & 0x02) |
| 234 | { // inverting movement of servo |
233 | { // inverting movement of servo |
| 235 | ServoRollValue += (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * roll ) / (256L) ); |
234 | ServoRollValue += roll; |
| 236 | } |
235 | } |
| 237 | else |
236 | else |
| 238 | { // non inverting movement of servo |
237 | { // non inverting movement of servo |
| 239 | ServoRollValue -= (int16_t)( ( (int32_t) EE_Parameter.ServoRollComp * MULTIPLYER * roll ) / (256L) ); |
238 | ServoRollValue -= roll; |
| 240 | } |
239 | } |
| 241 | // limit servo value to its parameter range definition |
240 | // limit servo value to its parameter range definition |
| 242 | if(ServoRollValue < ((int16_t)EE_Parameter.ServoRollMin * MULTIPLYER) ) |
241 | if(ServoRollValue < ((int16_t)EE_Parameter.ServoRollMin * MULTIPLYER) ) |
| 243 | { |
242 | { |
| 244 | ServoRollValue = (int16_t)EE_Parameter.ServoRollMin * MULTIPLYER; |
243 | ServoRollValue = (int16_t)EE_Parameter.ServoRollMin * MULTIPLYER; |