Rev 2164 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2164 | Rev 2189 | ||
---|---|---|---|
1 | #include <util/delay.h> |
1 | #include <util/delay.h> |
2 | #include <stddef.h> |
2 | #include <stddef.h> |
3 | #include <string.h> |
3 | #include <string.h> |
4 | #include "configuration.h" |
4 | #include "configuration.h" |
5 | #include "sensors.h" |
5 | #include "sensors.h" |
6 | #include "rc.h" |
6 | #include "rc.h" |
7 | #include "output.h" |
7 | #include "output.h" |
8 | #include "flight.h" |
8 | #include "flight.h" |
- | 9 | #include "debug.h" |
|
9 | 10 | ||
10 | int16_t variables[VARIABLE_COUNT]; |
11 | int16_t variables[VARIABLE_COUNT]; |
11 | ParamSet_t staticParams; |
12 | ParamSet_t staticParams; |
12 | ChannelMap_t channelMap; |
13 | ChannelMap_t channelMap; |
13 | MotorMixer_t motorMixer; |
14 | OutputMixer_t outputMixer; |
14 | IMUConfig_t IMUConfig; |
15 | IMUConfig_t IMUConfig; |
15 | volatile DynamicParams_t dynamicParams; |
16 | volatile DynamicParams_t dynamicParams; |
16 | 17 | ||
17 | uint8_t CPUType; |
18 | uint8_t CPUType; |
18 | uint8_t boardRelease; |
19 | uint8_t boardRelease; |
19 | uint8_t requiredMotors; |
- | |
20 | 20 | ||
21 | VersionInfo_t versionInfo; |
21 | VersionInfo_t versionInfo; |
22 | 22 | ||
23 | // MK flags. TODO: Replace by enum. State machine. |
23 | // MK flags. TODO: Replace by enum. State machine. |
24 | uint16_t isFlying = 0; |
24 | uint16_t isFlying; |
25 | volatile uint8_t MKFlags = 0; |
25 | volatile uint8_t MKFlags; |
26 | 26 | ||
- | 27 | const MMXLATION XLATIONS[] = { |
|
- | 28 | {offsetof(ParamSet_t, flightMode), offsetof(DynamicParams_t, flightMode),0,255}, |
|
27 | const MMXLATION XLATIONS[] = { |
29 | |
28 | {offsetof(ParamSet_t, levelCorrection[0]), offsetof(DynamicParams_t, levelCorrection[0]),0,255}, |
30 | {offsetof(ParamSet_t, levelCorrection[0]), offsetof(DynamicParams_t, levelCorrection[0]),0,255}, |
- | 31 | {offsetof(ParamSet_t, levelCorrection[1]), offsetof(DynamicParams_t, levelCorrection[1]),0,255}, |
|
29 | {offsetof(ParamSet_t, levelCorrection[1]), offsetof(DynamicParams_t, levelCorrection[1]),0,255}, |
32 | |
- | 33 | {offsetof(ParamSet_t, attGyroP), offsetof(DynamicParams_t, attGyroP),0,250}, |
|
- | 34 | {offsetof(ParamSet_t, attGyroI), offsetof(DynamicParams_t, attGyroI),0,250}, |
|
- | 35 | {offsetof(ParamSet_t, attGyroD), offsetof(DynamicParams_t, attGyroD),0,250}, |
|
- | 36 | ||
- | 37 | {offsetof(ParamSet_t, rateGyroP), offsetof(DynamicParams_t, rateGyroP),0,250}, |
|
- | 38 | {offsetof(ParamSet_t, rateGyroI), offsetof(DynamicParams_t, rateGyroI),0,250}, |
|
- | 39 | {offsetof(ParamSet_t, rateGyroD), offsetof(DynamicParams_t, rateGyroD),0,250}, |
|
- | 40 | ||
30 | {offsetof(ParamSet_t, gyroP), offsetof(DynamicParams_t, gyroP),0,255}, |
41 | {offsetof(ParamSet_t, yawGyroP), offsetof(DynamicParams_t, yawGyroP),0,250}, |
31 | {offsetof(ParamSet_t, gyroI), offsetof(DynamicParams_t, gyroI),0,255}, |
42 | {offsetof(ParamSet_t, yawGyroI), offsetof(DynamicParams_t, yawGyroI),0,250}, |
- | 43 | {offsetof(ParamSet_t, yawGyroD), offsetof(DynamicParams_t, yawGyroD),0,250}, |
|
32 | {offsetof(ParamSet_t, gyroD), offsetof(DynamicParams_t, gyroD),0,255}, |
44 | |
33 | {offsetof(ParamSet_t, attitudeControl), offsetof(DynamicParams_t, attitudeControl),0,255}, |
45 | {offsetof(ParamSet_t, attitudeControl), offsetof(DynamicParams_t, attitudeControl),0,255}, |
34 | {offsetof(ParamSet_t, externalControl), offsetof(DynamicParams_t, externalControl),0,255}, |
- | |
35 | {offsetof(ParamSet_t, dynamicStability), offsetof(DynamicParams_t, dynamicStability),0,255}, |
46 | {offsetof(ParamSet_t, externalControl), offsetof(DynamicParams_t, externalControl),0,255}, |
36 | {offsetof(ParamSet_t, heightP), offsetof(DynamicParams_t, heightP),0,255}, |
47 | {offsetof(ParamSet_t, heightP), offsetof(DynamicParams_t, heightP),0,255}, |
37 | {offsetof(ParamSet_t, heightI), offsetof(DynamicParams_t, heightI),0,255}, |
48 | {offsetof(ParamSet_t, heightI), offsetof(DynamicParams_t, heightI),0,255}, |
38 | {offsetof(ParamSet_t, heightD), offsetof(DynamicParams_t, heightD),0,255}, |
49 | {offsetof(ParamSet_t, heightD), offsetof(DynamicParams_t, heightD),0,255}, |
39 | {offsetof(ParamSet_t, heightSetting), offsetof(DynamicParams_t, heightSetting),0,255}, |
50 | {offsetof(ParamSet_t, heightSetting), offsetof(DynamicParams_t, heightSetting),0,255}, |
40 | {offsetof(ParamSet_t, servoConfigurations[0].manualControl), offsetof(DynamicParams_t, servoManualControl[0]),0,255}, |
51 | {offsetof(ParamSet_t, servoConfigurations[0].manualControl), offsetof(DynamicParams_t, servoManualControl[0]),0,255}, |
41 | {offsetof(ParamSet_t, servoConfigurations[1].manualControl), offsetof(DynamicParams_t, servoManualControl[1]),0,255}, |
52 | {offsetof(ParamSet_t, servoConfigurations[1].manualControl), offsetof(DynamicParams_t, servoManualControl[1]),0,255}, |
42 | {offsetof(ParamSet_t, outputFlash[0].timing), offsetof(DynamicParams_t, output0Timing),0,255}, |
53 | {offsetof(ParamSet_t, outputFlash[0].timing), offsetof(DynamicParams_t, output0Timing),0,255}, |
43 | {offsetof(ParamSet_t, outputFlash[1].timing), offsetof(DynamicParams_t, output1Timing),0,255}, |
54 | {offsetof(ParamSet_t, outputFlash[1].timing), offsetof(DynamicParams_t, output1Timing),0,255}, |
44 | {offsetof(ParamSet_t, naviMode), offsetof(DynamicParams_t, naviMode),0,255}}; |
55 | {offsetof(ParamSet_t, naviMode), offsetof(DynamicParams_t, naviMode),0,255}}; |
45 | 56 | ||
46 | uint8_t configuration_applyVariableToParam(uint8_t src, uint8_t min, uint8_t max) { |
57 | uint8_t configuration_applyVariableToParam(uint8_t src, uint8_t min, uint8_t max) { |
47 | uint8_t result; |
58 | uint8_t result; |
48 | if (src>=(256-VARIABLE_COUNT)) result = variables[src-(256-VARIABLE_COUNT)]; |
59 | if (src>=(256-VARIABLE_COUNT)) result = variables[src-(256-VARIABLE_COUNT)]; |
49 | else result = src; |
60 | else result = src; |
50 | if (result < min) result = min; |
61 | if (result < min) result = min; |
51 | else if (result > max) result = max; |
62 | else if (result > max) result = max; |
52 | return result; |
63 | return result; |
53 | } |
64 | } |
- | 65 | ||
- | 66 | void configuration_fixVariableParams(uint8_t variableNumber) { |
|
- | 67 | // If variable value is to great, limit it. |
|
- | 68 | uint8_t value = variables[variableNumber]; |
|
- | 69 | if (value >= 256-VARIABLE_COUNT) |
|
- | 70 | value = 256-VARIABLE_COUNT - 1; |
|
- | 71 | for (uint8_t i=0; i<sizeof(DynamicParams_t); i++) { |
|
- | 72 | uint8_t* pointerToParam =(uint8_t*)(&dynamicParams + i); |
|
- | 73 | if (*pointerToParam == 256-VARIABLE_COUNT + variableNumber) { |
|
- | 74 | *pointerToParam = value; |
|
- | 75 | } |
|
- | 76 | } |
|
- | 77 | } |
|
54 | 78 | ||
55 | void configuration_applyVariablesToParams(void) { |
79 | void configuration_applyVariablesToParams(void) { |
56 | uint8_t i, src; |
80 | uint8_t i, src; |
57 | uint8_t* pointerToTgt; |
81 | uint8_t* pointerToTgt; |
58 | 82 | ||
59 | for(i=0; i<sizeof(XLATIONS)/sizeof(MMXLATION); i++) { |
83 | for(i=0; i<sizeof(XLATIONS)/sizeof(MMXLATION); i++) { |
60 | src = *((uint8_t*)(&staticParams) + XLATIONS[i].sourceIdx); |
84 | src = *((uint8_t*)(&staticParams) + XLATIONS[i].sourceIdx); |
61 | pointerToTgt = (uint8_t*)(&dynamicParams) + XLATIONS[i].targetIdx; |
85 | pointerToTgt = (uint8_t*)(&dynamicParams) + XLATIONS[i].targetIdx; |
62 | *pointerToTgt = configuration_applyVariableToParam(src, XLATIONS[i].min, XLATIONS[i].max); |
86 | *pointerToTgt = configuration_applyVariableToParam(src, XLATIONS[i].min, XLATIONS[i].max); |
63 | } |
87 | } |
64 | 88 | ||
65 | // User parameters are always variable. |
89 | // User parameters are always variable. |
66 | for (i=0; i<sizeof(staticParams.userParams); i++) { |
90 | for (i=0; i<sizeof(staticParams.userParams); i++) { |
67 | src = *((uint8_t*)(&staticParams) + offsetof(ParamSet_t, userParams) + i); |
91 | src = *((uint8_t*)(&staticParams) + offsetof(ParamSet_t, userParams) + i); |
68 | pointerToTgt = (uint8_t*)(&dynamicParams) + offsetof(DynamicParams_t, userParams) + i; |
92 | pointerToTgt = (uint8_t*)(&dynamicParams) + offsetof(DynamicParams_t, userParams) + i; |
69 | *pointerToTgt = configuration_applyVariableToParam(src, 0, 255); |
93 | *pointerToTgt = configuration_applyVariableToParam(src, 0, 255); |
70 | } |
94 | } |
71 | } |
95 | } |
72 | 96 | ||
73 | void setCPUType(void) { // works only after reset or power on when the registers have default values |
97 | void setCPUType(void) { // works only after reset or power on when the registers have default values |
74 | #if (MCU_TYPE==atmega644) |
98 | #if (MCU_TYPE==atmega644) |
75 | CPUType=ATMEGA644; |
99 | CPUType=ATMEGA644; |
76 | #else |
100 | #else |
77 | if((UCSR1A == 0x20) && (UCSR1C == 0x06)) CPUType = ATMEGA644P; // initial Values for 644P after reset |
101 | if((UCSR1A == 0x20) && (UCSR1C == 0x06)) CPUType = ATMEGA644P; // initial Values for 644P after reset |
78 | else CPUType = ATMEGA644; |
102 | else CPUType = ATMEGA644; |
79 | #endif |
103 | #endif |
80 | } |
104 | } |
81 | 105 | ||
82 | /* |
106 | /* |
83 | * Automatic detection of hardware components is not supported in this development-oriented |
107 | * Automatic detection of hardware components is not supported in this development-oriented |
84 | * FC firmware. It would go against the point of it: To enable alternative hardware |
108 | * FC firmware. It would go against the point of it: To enable alternative hardware |
85 | * configurations with otherwise unsupported components. Instead, one should write |
109 | * configurations with otherwise unsupported components. Instead, one should write |
86 | * custom code + adjust constants for the new hardware, and include the relevant code |
110 | * custom code + adjust constants for the new hardware, and include the relevant code |
87 | * from the makefile. |
111 | * from the makefile. |
88 | * However - we still do detect the board release. Reason: Otherwise it would be too |
112 | * However - we still do detect the board release. Reason: Otherwise it would be too |
89 | * tedious to have to modify the code for how to turn on and off LEDs when deploying |
113 | * tedious to have to modify the code for how to turn on and off LEDs when deploying |
90 | * on different HW version.... |
114 | * on different HW version.... |
91 | */ |
115 | */ |
92 | void setBoardRelease(void) { |
116 | void setBoardRelease(void) { |
93 | // the board release is coded via the pull up or down the 2 status LED |
117 | // the board release is coded via the pull up or down the 2 status LED |
94 | 118 | ||
95 | PORTB &= ~((1 << PORTB1)|(1 << PORTB0)); // set tristate |
119 | PORTB &= ~((1 << PORTB1)|(1 << PORTB0)); // set tristate |
96 | DDRB &= ~((1 << DDB0)|(1 << DDB0)); // set port direction as input |
120 | DDRB &= ~((1 << DDB0)|(1 << DDB0)); // set port direction as input |
97 | 121 | ||
98 | _delay_loop_2(1000); // make some delay |
122 | _delay_loop_2(1000); // make some delay |
99 | 123 | ||
100 | switch( PINB & ((1<<PINB1)|(1<<PINB0))) { |
124 | switch( PINB & ((1<<PINB1)|(1<<PINB0))) { |
101 | case 0x00: |
125 | case 0x00: |
102 | boardRelease = 10; // 1.0 |
126 | boardRelease = 10; // 1.0 |
103 | break; |
127 | break; |
104 | case 0x01: |
128 | case 0x01: |
105 | boardRelease = 11; // 1.1 or 1.2 |
129 | boardRelease = 11; // 1.1 or 1.2 |
106 | break; |
130 | break; |
107 | case 0x02: |
131 | case 0x02: |
108 | boardRelease = 20; // 2.0 |
132 | boardRelease = 20; // 2.0 |
109 | break; |
133 | break; |
110 | case 0x03: |
134 | case 0x03: |
111 | boardRelease = 13; // 1.3 |
135 | boardRelease = 13; // 1.3 |
112 | break; |
136 | break; |
113 | default: |
137 | default: |
114 | break; |
138 | break; |
115 | } |
139 | } |
116 | // set LED ports as output |
140 | // set LED ports as output |
117 | DDRB |= (1<<DDB1)|(1<<DDB0); |
141 | DDRB |= (1<<DDB1)|(1<<DDB0); |
118 | RED_OFF; |
142 | RED_OFF; |
119 | GRN_OFF; |
143 | GRN_OFF; |
120 | } |
144 | } |
121 | 145 | ||
- | 146 | void configuration_setNormalFlightMode(void) { |
|
- | 147 | FlightMode_t flight_flightMode; |
|
- | 148 | if (dynamicParams.flightMode < 255/3) |
|
122 | void configuration_setNormalFlightParameters(void) { |
149 | flight_flightMode = FM_RETURN_TO_LEVEL; |
123 | flight_setParameters(staticParams.IFactor, dynamicParams.gyroP, |
150 | else if (dynamicParams.flightMode < 255*2/3) |
- | 151 | flight_flightMode = FM_HEADING_HOLD; |
|
- | 152 | else |
|
- | 153 | flight_flightMode = FM_RATE; |
|
124 | staticParams.bitConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.gyroI, |
154 | flight_setMode(flight_flightMode); |
125 | dynamicParams.gyroP, staticParams.yawIFactor); |
155 | debugOut.analog[20] = flight_flightMode; |
126 | } |
156 | } |
127 | 157 | ||
128 | void configuration_setFailsafeFlightParameters(void) { |
158 | void configuration_setFailsafeFlightMode(void) { |
129 | flight_setParameters(0, 90, 120, 90, 120); |
159 | flight_setMode(FM_RETURN_TO_LEVEL); |
130 | } |
160 | } |
131 | 161 | ||
132 | // Called after a change in configuration parameters, as a hook for modules to take over changes. |
162 | // Called after a change in configuration parameters, as a hook for modules to take over changes. |
133 | void configuration_paramSetDidChange(void) { |
163 | void configuration_paramSetDidChange(void) { |
134 | // This should be OK to do here as long as we don't change parameters during emergency flight. We don't. |
164 | // This should be OK to do here as long as we don't change parameters during emergency flight. We don't. |
- | 165 | // No longer necessary. |
|
135 | configuration_setNormalFlightParameters(); |
166 | // configuration_setNormalFlightMode(); |
- | 167 | flight_setParameters(); |
|
- | 168 | output_setParameters(); |
|
136 | // Immediately load changes to output, and also signal the paramset change. |
169 | // Immediately load changes to output, and also signal the paramset change. |
137 | output_init(); |
170 | output_init(); |
138 | } |
171 | } |
139 | 172 | ||
140 | void setOtherDefaults(void) { |
173 | void setOtherDefaults(void) { |
141 | // Height Control |
174 | // Height Control |
142 | staticParams.airpressureFilterConstant = 8; |
175 | staticParams.airpressureFilterConstant = 8; |
143 | //staticParams.airpressureWindowLength = 0; |
176 | staticParams.airpressureWindowLength = 8; |
- | 177 | staticParams.airpressureDWindowLength = 24; |
|
- | 178 | ||
144 | staticParams.airpressureAccZCorrection = 128+56; |
179 | staticParams.airpressureAccZCorrection = 128+56; |
145 | staticParams.heightP = 10; |
180 | staticParams.heightP = 10; |
146 | staticParams.heightD = 30; |
181 | staticParams.heightD = 30; |
147 | staticParams.heightSetting = 251; |
182 | staticParams.heightSetting = 251; |
148 | staticParams.heightControlMaxThrottleChange = 10; |
183 | staticParams.heightControlMaxThrottleChange = 10; |
149 | 184 | ||
150 | // Control |
185 | // Control |
- | 186 | staticParams.flightMode = 248; |
|
151 | staticParams.stickP = 8; |
187 | staticParams.stickP = 8; |
152 | staticParams.stickD = 12; |
188 | staticParams.stickD = 0; |
153 | staticParams.stickYawP = 12; |
189 | staticParams.stickYawP = 8; |
154 | staticParams.stickThrottleD = 12; |
190 | staticParams.stickThrottleD = 4; |
- | 191 | ||
155 | staticParams.minThrottle = 8; |
192 | staticParams.minThrottle = 8; |
156 | staticParams.maxThrottle = 230; |
193 | staticParams.maxThrottle = 230; |
- | 194 | ||
157 | staticParams.externalControl = 0; |
195 | staticParams.externalControl = 0; |
158 | staticParams.attitudeControl = 0; |
196 | staticParams.attitudeControl = 0; |
159 | staticParams.motorSmoothing = 0; |
197 | staticParams.dynamicStability = 80; // 0 means: Regulation only by decreasing throttle. 200: Only by increasing. |
160 | - | ||
161 | staticParams.gyroP = 60; |
- | |
162 | staticParams.gyroI = 80; |
- | |
163 | staticParams.gyroD = 4; |
- | |
164 | 198 | ||
165 | // set by gyro-specific code: gyro_setDefaults(). |
199 | staticParams.attGyroP = PID_NORMAL_VALUE; |
166 | // staticParams.zerothOrderCorrection = |
200 | staticParams.attGyroI = PID_NORMAL_VALUE; |
- | 201 | staticParams.attGyroIMax = 0; |
|
- | 202 | staticParams.attGyroD = PID_NORMAL_VALUE * 3/2; |
|
- | 203 | ||
- | 204 | staticParams.rateGyroP = PID_NORMAL_VALUE; |
|
167 | // staticParams.driftCompDivider = |
205 | staticParams.rateGyroI = PID_NORMAL_VALUE; |
- | 206 | staticParams.rateGyroIMax = 0; |
|
- | 207 | staticParams.rateGyroD = PID_NORMAL_VALUE; |
|
- | 208 | ||
- | 209 | staticParams.yawGyroP = PID_NORMAL_VALUE; |
|
- | 210 | staticParams.yawGyroI = PID_NORMAL_VALUE; |
|
168 | // staticParams.driftCompLimit = |
- | |
169 | 211 | staticParams.yawGyroD = PID_NORMAL_VALUE; |
|
170 | staticParams.dynamicStability = 50; |
- | |
171 | staticParams.IFactor = 52; |
212 | |
172 | staticParams.yawIFactor = 100; |
213 | staticParams.compassMode = 0; |
173 | staticParams.compassYawCorrection = 64; |
214 | staticParams.compassYawCorrection = 64; |
- | 215 | staticParams.compassP = PID_NORMAL_VALUE; |
|
174 | staticParams.compassP = 50; |
216 | staticParams.levelCorrection[0] = 249; // var1 |
175 | staticParams.levelCorrection[0] = staticParams.levelCorrection[1] = 128; |
217 | staticParams.levelCorrection[1] = 250; // var2 |
176 | 218 | ||
177 | // Servos |
219 | // Servos |
178 | staticParams.servoCount = 7; |
220 | staticParams.servoCount = 8; |
179 | staticParams.servoManualMaxSpeed = 10; |
221 | staticParams.servoManualMaxSpeed = 10; |
180 | for (uint8_t i=0; i<2; i++) { |
222 | for (uint8_t i=0; i<2; i++) { |
181 | staticParams.servoConfigurations[i].manualControl = 128; |
223 | staticParams.servoConfigurations[i].manualControl = 128; |
182 | staticParams.servoConfigurations[i].stabilizationFactor = 0; |
224 | staticParams.servoConfigurations[i].stabilizationFactor = 100; |
183 | staticParams.servoConfigurations[i].minValue = 32; |
- | |
184 | staticParams.servoConfigurations[i].maxValue = 224; |
- | |
185 | staticParams.servoConfigurations[i].flags = 0; |
225 | staticParams.servoConfigurations[i].flags = 0; |
186 | } |
226 | } |
187 | 227 | ||
188 | // Battery warning and emergency flight |
228 | // Battery warning and emergency flight |
189 | staticParams.batteryVoltageWarning = 101; // 3.7 each for 3S |
229 | staticParams.batteryVoltageWarning = 101; // 3.7 each for 3S |
190 | staticParams.emergencyThrottle = 35; |
230 | staticParams.emergencyThrottle = 35; |
191 | staticParams.emergencyFlightDuration = 30; |
231 | staticParams.emergencyFlightDuration = 30; |
192 | 232 | ||
193 | // Outputs |
233 | // Outputs |
194 | staticParams.outputFlash[0].bitmask = 1; //0b01011111; |
234 | staticParams.outputFlash[0].bitmask = 1; //0b01011111; |
195 | staticParams.outputFlash[0].timing = 15; |
235 | staticParams.outputFlash[0].timing = 15; |
196 | staticParams.outputFlash[1].bitmask = 3; //0b11110011; |
236 | staticParams.outputFlash[1].bitmask = 3; //0b11110011; |
197 | staticParams.outputFlash[1].timing = 15; |
237 | staticParams.outputFlash[1].timing = 15; |
198 | 238 | ||
199 | staticParams.outputDebugMask = DEBUG_ACC0THORDER; |
239 | staticParams.outputDebugMask = DEBUG_INVERTED; |
200 | staticParams.outputFlags = OUTPUTFLAGS_FLASH_0_AT_BEEP | OUTPUTFLAGS_FLASH_1_AT_BEEP | OUTPUTFLAGS_USE_ONBOARD_LEDS; |
240 | staticParams.outputFlags = /* OUTPUTFLAGS_FLASH_0_AT_BEEP | OUTPUTFLAGS_FLASH_1_AT_BEEP | */ OUTPUTFLAGS_USE_ONBOARD_LEDS; |
201 | - | ||
202 | staticParams.naviMode = 0; // free. |
- | |
203 | staticParams.airpressureWindowLength = 0; |
241 | |
204 | staticParams.airpressureDWindowLength = 24; |
242 | staticParams.naviMode = 0; // free. |
205 | 243 | ||
206 | staticParams.heightControlMaxIntegralIn = 125; |
244 | staticParams.heightControlMaxIntegralIn = 125; |
207 | staticParams.heightControlMaxIntegralOut = 75; |
245 | staticParams.heightControlMaxIntegralOut = 75; |
208 | staticParams.heightControlMaxThrottleChange = 75; |
246 | staticParams.heightControlMaxThrottleChange = 75; |
209 | staticParams.heightControlTestOscPeriod = 0; |
247 | staticParams.heightControlTestOscPeriod = 0; |
210 | staticParams.heightControlTestOscAmplitude = 0; |
248 | staticParams.heightControlTestOscAmplitude = 0; |
211 | } |
249 | } |
212 | 250 | ||
213 | /***************************************************/ |
251 | /***************************************************/ |
214 | /* Default Values for parameter set 1 */ |
252 | /* Default Values for parameter set 1 */ |
215 | /***************************************************/ |
253 | /***************************************************/ |
216 | void paramSet_default(uint8_t setnumber) { |
254 | void paramSet_default(uint8_t setnumber) { |
217 | setOtherDefaults(); |
255 | setOtherDefaults(); |
218 | 256 | ||
219 | for (uint8_t i=0; i<8; i++) { |
257 | for (uint8_t i=0; i<8; i++) { |
220 | staticParams.userParams[i] = i; |
258 | staticParams.userParams[i] = i; |
221 | } |
259 | } |
222 | 260 | ||
223 | staticParams.bitConfig = CFG_GYRO_SATURATION_PREVENTION; |
261 | staticParams.bitConfig = 0;//CFG_GYRO_SATURATION_PREVENTION; |
224 | 262 | ||
225 | memcpy(staticParams.name, "Default\0", 6); |
263 | memcpy(staticParams.name, "Default\0", 6); |
226 | } |
264 | } |
227 | 265 | ||
228 | void IMUConfig_default(void) { |
266 | void IMUConfig_default(void) { |
229 | IMUConfig.gyroPIDFilterConstant = 1; |
267 | IMUConfig.gyroPIDFilterConstant = 1; |
230 | IMUConfig.gyroDWindowLength = 3; |
268 | IMUConfig.gyroDWindowLength = 3; |
231 | // IMUConfig.gyroDFilterConstant = 1; |
269 | // IMUConfig.gyroDFilterConstant = 1; |
232 | IMUConfig.accFilterConstant = 10; |
270 | IMUConfig.accFilterConstant = 10; |
233 | IMUConfig.rateTolerance = 120; |
- | |
234 | IMUConfig.gyroActivityDamping = 24; |
- | |
- | 271 | ||
235 | 272 | ||
236 | gyro_setDefaultParameters(); |
273 | gyro_setDefaultParameters(); |
237 | } |
274 | } |
238 | 275 | ||
239 | /***************************************************/ |
276 | /***************************************************/ |
240 | /* Default Values for Mixer Table */ |
277 | /* Default Values for Mixer Table */ |
241 | /***************************************************/ |
278 | /***************************************************/ |
242 | void motorMixer_default(void) { // Quadro |
279 | void outputMixer_default(void) { // Quadro |
243 | uint8_t i; |
280 | uint8_t i; |
244 | // clear mixer table (but preset throttle) |
281 | // clear mixer table (but preset throttle) |
245 | for (i = 0; i < MAX_MOTORS; i++) { |
282 | for (i = 0; i < NUM_OUTPUTS; i++) { |
246 | motorMixer.matrix[i][MIX_THROTTLE] = i < 4 ? 64 : 0; |
283 | outputMixer[i].flightControls[MIXER_SOURCE_THROTTLE] = i < 4 ? (1<<LOG_MOTOR_MIXER_UNIT) : 0; |
247 | motorMixer.matrix[i][MIX_PITCH] = 0; |
284 | outputMixer[i].flightControls[MIXER_SOURCE_PITCH] = 0; |
248 | motorMixer.matrix[i][MIX_ROLL] = 0; |
285 | outputMixer[i].flightControls[MIXER_SOURCE_ROLL] = 0; |
249 | motorMixer.matrix[i][MIX_YAW] = 0; |
286 | outputMixer[i].flightControls[MIXER_SOURCE_YAW] = 0; |
250 | motorMixer.matrix[i][MIX_OPPOSITE_MOTOR] = (uint8_t)-1; |
287 | outputMixer[i].oppositeMotor = (uint8_t)-1; |
- | 288 | outputMixer[i].auxSource = (uint8_t)-1; |
|
- | 289 | outputMixer[i].outputType = i < 4 ? OUTPUT_TYPE_MOTOR : OUTPUT_TYPE_UNDEFINED; |
|
- | 290 | outputMixer[i].minValue = i < 4 ? 1 : 128; |
|
- | 291 | outputMixer[i].maxValue = i < 4 ? 255 : 128; |
|
251 | } |
292 | } |
252 | /* |
- | |
253 | // default = Quadro+ |
293 | // default = Quadro+ |
254 | motorMixer.matrix[0][MIX_PITCH] = +64; |
294 | outputMixer[0].flightControls[MIXER_SOURCE_PITCH] = -(1<<LOG_MOTOR_MIXER_UNIT); |
255 | motorMixer.matrix[0][MIX_YAW] = +64; |
295 | outputMixer[0].flightControls[MIXER_SOURCE_YAW] = +(1<<LOG_MOTOR_MIXER_UNIT); |
256 | motorMixer.matrix[0][MIX_OPPOSITE_MOTOR] = 1; |
296 | outputMixer[0].oppositeMotor = 1; |
257 | 297 | ||
258 | motorMixer.matrix[1][MIX_PITCH] = -64; |
298 | outputMixer[1].flightControls[MIXER_SOURCE_PITCH] = +(1<<LOG_MOTOR_MIXER_UNIT); |
259 | motorMixer.matrix[1][MIX_YAW] = +64; |
299 | outputMixer[1].flightControls[MIXER_SOURCE_YAW] = +(1<<LOG_MOTOR_MIXER_UNIT); |
260 | motorMixer.matrix[1][MIX_OPPOSITE_MOTOR] = 0; |
300 | outputMixer[1].oppositeMotor = 0; |
261 | 301 | ||
262 | motorMixer.matrix[2][MIX_ROLL] = -64; |
302 | outputMixer[2].flightControls[MIXER_SOURCE_ROLL] = +(1<<LOG_MOTOR_MIXER_UNIT); |
263 | motorMixer.matrix[2][MIX_YAW] = -64; |
303 | outputMixer[2].flightControls[MIXER_SOURCE_YAW] = -(1<<LOG_MOTOR_MIXER_UNIT); |
264 | motorMixer.matrix[2][MIX_OPPOSITE_MOTOR] = 3; |
304 | outputMixer[2].oppositeMotor = 3; |
265 | 305 | ||
266 | motorMixer.matrix[3][MIX_ROLL] = +64; |
306 | outputMixer[3].flightControls[MIXER_SOURCE_ROLL] = -(1<<LOG_MOTOR_MIXER_UNIT); |
267 | motorMixer.matrix[3][MIX_YAW] = -64; |
307 | outputMixer[3].flightControls[MIXER_SOURCE_YAW] = -(1<<LOG_MOTOR_MIXER_UNIT); |
268 | motorMixer.matrix[3][MIX_OPPOSITE_MOTOR] = 2; |
308 | outputMixer[3].oppositeMotor = 2; |
269 | 309 | ||
- | 310 | outputMixer[8].outputType = OUTPUT_TYPE_SERVO; |
|
- | 311 | outputMixer[8].auxSource = MIXER_SOURCE_AUX_RCCHANNEL; |
|
- | 312 | outputMixer[8].minValue = 40; |
|
270 | memcpy(motorMixer.name, "Quadro +\0", 9); |
313 | outputMixer[8].maxValue = 256-40; |
271 | */ |
314 | |
- | 315 | outputMixer[9].outputType = OUTPUT_TYPE_SERVO; |
|
- | 316 | outputMixer[9].auxSource = MIXER_SOURCE_AUX_RCCHANNEL+1; |
|
- | 317 | outputMixer[9].minValue = 10; |
|
- | 318 | outputMixer[9].maxValue = 256-10; |
|
272 | 319 | ||
- | 320 | /* |
|
273 | // default = Quadro |
321 | // default = Quadro X |
274 | motorMixer.matrix[0][MIX_PITCH] = +64; |
322 | motorMixer.matrix[0][MIX_PITCH] = +(1<<LOG_MOTOR_MIXER_SCALER); |
275 | motorMixer.matrix[0][MIX_ROLL] = +64; |
323 | motorMixer.matrix[0][MIX_ROLL] = +(1<<LOG_MOTOR_MIXER_SCALER); |
276 | motorMixer.matrix[0][MIX_YAW] = +64; |
324 | motorMixer.matrix[0][MIX_YAW] = +(1<<LOG_MOTOR_MIXER_SCALER); |
277 | motorMixer.matrix[0][MIX_OPPOSITE_MOTOR] = 1; |
325 | motorMixer.matrix[0][MIX_OPPOSITE_MOTOR] = 1; |
278 | 326 | ||
279 | motorMixer.matrix[1][MIX_PITCH] = -64; |
327 | motorMixer.matrix[1][MIX_PITCH] = -(1<<LOG_MOTOR_MIXER_SCALER); |
280 | motorMixer.matrix[1][MIX_ROLL] = -64; |
328 | motorMixer.matrix[1][MIX_ROLL] = -(1<<LOG_MOTOR_MIXER_SCALER); |
281 | motorMixer.matrix[1][MIX_YAW] = +64; |
329 | motorMixer.matrix[1][MIX_YAW] = +(1<<LOG_MOTOR_MIXER_SCALER); |
282 | motorMixer.matrix[1][MIX_OPPOSITE_MOTOR] = 0; |
330 | motorMixer.matrix[1][MIX_OPPOSITE_MOTOR] = 0; |
283 | 331 | ||
284 | motorMixer.matrix[2][MIX_PITCH] = +64; |
332 | motorMixer.matrix[2][MIX_PITCH] = +(1<<LOG_MOTOR_MIXER_SCALER); |
285 | motorMixer.matrix[2][MIX_ROLL] = -64; |
333 | motorMixer.matrix[2][MIX_ROLL] = -(1<<LOG_MOTOR_MIXER_SCALER); |
286 | motorMixer.matrix[2][MIX_YAW] = -64; |
334 | motorMixer.matrix[2][MIX_YAW] = -(1<<LOG_MOTOR_MIXER_SCALER); |
287 | motorMixer.matrix[2][MIX_OPPOSITE_MOTOR] = 3; |
335 | motorMixer.matrix[2][MIX_OPPOSITE_MOTOR] = 3; |
288 | 336 | ||
289 | motorMixer.matrix[3][MIX_PITCH] = -64; |
337 | motorMixer.matrix[3][MIX_PITCH] = -(1<<LOG_MOTOR_MIXER_SCALER); |
290 | motorMixer.matrix[3][MIX_ROLL] = +64; |
338 | motorMixer.matrix[3][MIX_ROLL] = +(1<<LOG_MOTOR_MIXER_SCALER); |
291 | motorMixer.matrix[3][MIX_YAW] = -64; |
339 | motorMixer.matrix[3][MIX_YAW] = -(1<<LOG_MOTOR_MIXER_SCALER); |
292 | motorMixer.matrix[3][MIX_OPPOSITE_MOTOR] = 2; |
340 | motorMixer.matrix[3][MIX_OPPOSITE_MOTOR] = 2; |
293 | 341 | ||
294 | memcpy(motorMixer.name, "Quadro X\0", 9); |
342 | memcpy(motorMixer.name, "Quadro X\0", 9); |
- | 343 | */ |
|
295 | } |
344 | } |
296 | 345 | ||
297 | /***************************************************/ |
346 | /***************************************************/ |
298 | /* Default Values for R/C Channels */ |
347 | /* Default Values for R/C Channels */ |
299 | /***************************************************/ |
348 | /***************************************************/ |
300 | void channelMap_default(void) { |
349 | void channelMap_default(void) { |
301 | channelMap.RCPolarity = 0; |
350 | channelMap.RCPolarity = 0; |
302 | channelMap.channels[CH_PITCH] = 1; |
351 | channelMap.channels[CH_PITCH] = 1; |
303 | channelMap.channels[CH_ROLL] = 0; |
352 | channelMap.channels[CH_ROLL] = 0; |
304 | channelMap.channels[CH_THROTTLE] = 2; |
353 | channelMap.channels[CH_THROTTLE] = 2; |
305 | channelMap.channels[CH_YAW] = 3; |
354 | channelMap.channels[CH_YAW] = 3; |
306 | channelMap.channels[CH_POTS + 0] = 4; |
355 | channelMap.channels[CH_VARIABLES + 0] = 4; |
307 | channelMap.channels[CH_POTS + 1] = 5; |
356 | channelMap.channels[CH_VARIABLES + 1] = 5; |
308 | channelMap.channels[CH_POTS + 2] = 6; |
357 | channelMap.channels[CH_VARIABLES + 2] = 6; |
309 | channelMap.channels[CH_POTS + 3] = 7; |
358 | channelMap.channels[CH_VARIABLES + 3] = 7; |
310 | } |
359 | } |
311 | 360 |