Subversion Repositories FlightCtrl

Rev

Rev 1078 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1078 Rev 1104
Line 3... Line 3...
3
#include "fc.h"
3
#include "fc.h"
4
#include "eeprom.h"
4
#include "eeprom.h"
5
#include "uart.h"
5
#include "uart.h"
6
#include "main.h"
6
#include "main.h"
Line 7... Line 7...
7
 
7
 
Line 8... Line 8...
8
volatile uint16_t ServoValue = 0;
8
volatile int16_t ServoValue = 0;
9
 
9
 
Line 80... Line 80...
80
                switch(ServoState)
80
                switch(ServoState)
81
                {
81
                {
82
                        case 4:
82
                        case 4:
83
                                // recalculate new ServoValue
83
                                // recalculate new ServoValue
84
                                ServoValue = 0x0030; // Offset (part 1)
84
                                ServoValue = 0x0030; // Offset (part 1)
-
 
85
                                DebugOut.Analog[16] = ServoValue;
85
                                FilterServo = (3 * FilterServo + (uint16_t)FCParam.ServoNickControl * 2) / 4; // lowpass static offset
86
                                FilterServo = (3 * FilterServo + (uint16_t)FCParam.ServoNickControl * 2) / 4; // lowpass static offset
86
                                ServoValue += FilterServo; // add filtered static offset
87
                                ServoValue += FilterServo; // add filtered static offset
Line 87... Line 88...
87
 
88
 
88
                                if(ParamSet.ServoNickCompInvert & 0x01)
89
                                if(ParamSet.ServoNickCompInvert & 0x01)
89
                                {       // inverting movement of servo
90
                                {       // inverting movement of servo
90
                                        ServoValue += ((int32_t) ((int32_t)ParamSet.ServoNickComp * IntegralNick) / 128L )/ (512L/MULTIPLIER);
91
                                        ServoValue += (int16_t)( ( (int32_t)ParamSet.ServoNickComp * (IntegralNick / 128L ) ) / (512L/MULTIPLIER) );
91
                                }
92
                                }
92
                                else
93
                                else
93
                                {       // non inverting movement of servo
94
                                {       // non inverting movement of servo
94
                                        ServoValue -= ((int32_t) ((int32_t)ParamSet.ServoNickComp * IntegralNick) / 128L) / (512L/MULTIPLIER);
95
                                        ServoValue -= (int16_t)( ( (int32_t)ParamSet.ServoNickComp * (IntegralNick / 128L ) ) / (512L/MULTIPLIER) );
Line 95... Line 96...
95
                                }
96
                                }
96
 
97
 
97
                                // limit servo value to its parameter range definition
98
                                // limit servo value to its parameter range definition
98
                                if(ServoValue < ((uint16_t)ParamSet.ServoNickMin * 3) )
99
                                if(ServoValue < ((int16_t)ParamSet.ServoNickMin * 3) )
99
                                {
100
                                {
100
                                        ServoValue = (uint16_t)ParamSet.ServoNickMin * 3;
101
                                        ServoValue = (int16_t)ParamSet.ServoNickMin * 3;
101
                                }
102
                                }
102
                                else
103
                                else
103
                                if(ServoValue > ((uint16_t)ParamSet.ServoNickMax * 3) )
104
                                if(ServoValue > ((int16_t)ParamSet.ServoNickMax * 3) )
104
                                {
105
                                {
Line 105... Line 106...
105
                                        ServoValue = (uint16_t)ParamSet.ServoNickMax * 3;
106
                                        ServoValue = (int16_t)ParamSet.ServoNickMax * 3;
106
                                }
107
                                }
107
 
108
 
108
                                DebugOut.Analog[20] = ServoValue;
109
                                DebugOut.Analog[20] = ServoValue;
109
                                // determine prepulse width (remaining part of ServoValue/Timer Cycle)
110
                                // determine prepulse width (remaining part of ServoValue/Timer Cycle)
110
                                if ((ServoValue % 255) < 45)
111
                                if ((ServoValue % 255) < 45)
111
                                {       // if prepulse width is to short the execution time of thios isr is longer than the next compare match
112
                                {       // if prepulse width is to short the execution time of this ISR is longer than the next compare match
112
                                        // so balance with postpulse width
113
                                        // so balance with postpulse width
113
                                        ServoValue += 77;
114
                                        ServoValue += 77;