root / quad1 / AeroQuad / DataStorage.h @ 9240aaa3
History | View | Annotate | Download (9.82 KB)
1 |
/*
|
---|---|
2 |
AeroQuad v2.1 - October 2010
|
3 |
www.AeroQuad.com
|
4 |
Copyright (c) 2010 Ted Carancho. All rights reserved.
|
5 |
An Open Source Arduino based multicopter.
|
6 |
|
7 |
This program is free software: you can redistribute it and/or modify
|
8 |
it under the terms of the GNU General Public License as published by
|
9 |
the Free Software Foundation, either version 3 of the License, or
|
10 |
(at your option) any later version.
|
11 |
|
12 |
This program is distributed in the hope that it will be useful,
|
13 |
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
14 |
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
15 |
GNU General Public License for more details.
|
16 |
|
17 |
You should have received a copy of the GNU General Public License
|
18 |
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
19 |
*/
|
20 |
|
21 |
// Utilities for writing and reading from the EEPROM
|
22 |
float readFloat(int address) { |
23 |
union floatStore {
|
24 |
byte floatByte[4];
|
25 |
float floatVal;
|
26 |
} floatOut; |
27 |
|
28 |
for (int i = 0; i < 4; i++) |
29 |
floatOut.floatByte[i] = EEPROM.read(address + i); |
30 |
return floatOut.floatVal;
|
31 |
} |
32 |
|
33 |
void writeFloat(float value, int address) { |
34 |
union floatStore {
|
35 |
byte floatByte[4];
|
36 |
float floatVal;
|
37 |
} floatIn; |
38 |
|
39 |
floatIn.floatVal = value; |
40 |
for (int i = 0; i < 4; i++) |
41 |
EEPROM.write(address + i, floatIn.floatByte[i]); |
42 |
} |
43 |
|
44 |
// contains all default values when re-writing EEPROM
|
45 |
void initializeEEPROM(void) { |
46 |
PID[ROLL].P = 1.2; |
47 |
PID[ROLL].I = 0.0; |
48 |
PID[ROLL].D = -7.0; |
49 |
PID[PITCH].P = 1.2; |
50 |
PID[PITCH].I = 0.0; |
51 |
PID[PITCH].D = -7.0; |
52 |
PID[YAW].P = 3.0; |
53 |
PID[YAW].I = 0.0; |
54 |
PID[YAW].D = 0.0; |
55 |
PID[LEVELROLL].P = 7.0; |
56 |
PID[LEVELROLL].I = 20.0; |
57 |
PID[LEVELROLL].D = 0.0; |
58 |
PID[LEVELPITCH].P = 7.0; |
59 |
PID[LEVELPITCH].I = 20.0; |
60 |
PID[LEVELPITCH].D = 0.0; |
61 |
PID[HEADING].P = 3.0; |
62 |
PID[HEADING].I = 0.0; |
63 |
PID[HEADING].D = 0.0; |
64 |
PID[LEVELGYROROLL].P = 1.2; |
65 |
PID[LEVELGYROROLL].I = 0.0; |
66 |
PID[LEVELGYROROLL].D = -14.0; |
67 |
PID[LEVELGYROPITCH].P = 1.2; |
68 |
PID[LEVELGYROPITCH].I = 0.0; |
69 |
PID[LEVELGYROPITCH].D = -14.0; |
70 |
#ifdef AltitudeHold
|
71 |
PID[ALTITUDE].P = 25.0; |
72 |
PID[ALTITUDE].I = 0.0; |
73 |
PID[ALTITUDE].D = 0.0; |
74 |
PID[ZDAMPENING].P = 0.0; |
75 |
PID[ZDAMPENING].I = 0.0; |
76 |
PID[ZDAMPENING].D = 0.0; |
77 |
minThrottleAdjust = -50.0; |
78 |
maxThrottleAdjust = 500.0; |
79 |
altitude.setSmoothFactor(0.1); |
80 |
#endif
|
81 |
|
82 |
#ifdef SonarHold
|
83 |
PID[SONAR].P = 25.0; |
84 |
PID[SONAR].I = 0.0; |
85 |
PID[SONAR].D = 0.0; //findtag |
86 |
sonarMinThrottleAdjust = -150.0; |
87 |
sonarMaxThrottleAdjust = 150.0; |
88 |
#endif
|
89 |
|
90 |
windupGuard = 1000.0; |
91 |
receiver.setXmitFactor(0.60); |
92 |
levelLimit = 500.0; |
93 |
levelOff = 150.0; |
94 |
gyro.setSmoothFactor(1.0); |
95 |
accel.setSmoothFactor(1.0); |
96 |
accel.setOneG(500);
|
97 |
timeConstant = 7.0; |
98 |
for (channel = ROLL; channel < LASTCHANNEL; channel++) {
|
99 |
receiver.setTransmitterSlope(channel, 1.0); |
100 |
receiver.setTransmitterOffset(channel, 0.0); |
101 |
} |
102 |
receiver.setSmoothFactor(THROTTLE, 1.0); |
103 |
receiver.setSmoothFactor(ROLL, 1.0); |
104 |
receiver.setSmoothFactor(PITCH, 1.0); |
105 |
receiver.setSmoothFactor(YAW, 0.5); |
106 |
receiver.setSmoothFactor(MODE, 1.0); |
107 |
receiver.setSmoothFactor(AUX, 1.0); |
108 |
smoothHeading = 1.0; |
109 |
flightMode = ACRO; |
110 |
headingHoldConfig = OFF; |
111 |
minAcro = 1300;
|
112 |
aref = 5.0; // Use 3.0 if using a v1.7 shield or use 2.8 for an AeroQuad Shield < v1.7 |
113 |
} |
114 |
|
115 |
void readEEPROM(void) { |
116 |
PID[ROLL].P = readFloat(PGAIN_ADR); |
117 |
PID[ROLL].I = readFloat(IGAIN_ADR); |
118 |
PID[ROLL].D = readFloat(DGAIN_ADR); |
119 |
PID[ROLL].lastPosition = 0;
|
120 |
PID[ROLL].integratedError = 0;
|
121 |
|
122 |
PID[PITCH].P = readFloat(PITCH_PGAIN_ADR); |
123 |
PID[PITCH].I = readFloat(PITCH_IGAIN_ADR); |
124 |
PID[PITCH].D = readFloat(PITCH_DGAIN_ADR); |
125 |
PID[PITCH].lastPosition = 0;
|
126 |
PID[PITCH].integratedError = 0;
|
127 |
|
128 |
PID[YAW].P = readFloat(YAW_PGAIN_ADR); |
129 |
PID[YAW].I = readFloat(YAW_IGAIN_ADR); |
130 |
PID[YAW].D = readFloat(YAW_DGAIN_ADR); |
131 |
PID[YAW].lastPosition = 0;
|
132 |
PID[YAW].integratedError = 0;
|
133 |
|
134 |
PID[LEVELROLL].P = readFloat(LEVEL_PGAIN_ADR); |
135 |
PID[LEVELROLL].I = readFloat(LEVEL_IGAIN_ADR); |
136 |
PID[LEVELROLL].D = readFloat(LEVEL_DGAIN_ADR); |
137 |
PID[LEVELROLL].lastPosition = 0;
|
138 |
PID[LEVELROLL].integratedError = 0;
|
139 |
|
140 |
PID[LEVELPITCH].P = readFloat(LEVEL_PITCH_PGAIN_ADR); |
141 |
PID[LEVELPITCH].I = readFloat(LEVEL_PITCH_IGAIN_ADR); |
142 |
PID[LEVELPITCH].D = readFloat(LEVEL_PITCH_DGAIN_ADR); |
143 |
PID[LEVELPITCH].lastPosition = 0;
|
144 |
PID[LEVELPITCH].integratedError = 0;
|
145 |
|
146 |
PID[HEADING].P = readFloat(HEADING_PGAIN_ADR); |
147 |
PID[HEADING].I = readFloat(HEADING_IGAIN_ADR); |
148 |
PID[HEADING].D = readFloat(HEADING_DGAIN_ADR); |
149 |
PID[HEADING].lastPosition = 0;
|
150 |
PID[HEADING].integratedError = 0;
|
151 |
|
152 |
PID[LEVELGYROROLL].P = readFloat(LEVEL_GYRO_ROLL_PGAIN_ADR); |
153 |
PID[LEVELGYROROLL].I = readFloat(LEVEL_GYRO_ROLL_IGAIN_ADR); |
154 |
PID[LEVELGYROROLL].D = readFloat(LEVEL_GYRO_ROLL_DGAIN_ADR); |
155 |
PID[LEVELGYROROLL].lastPosition = 0;
|
156 |
PID[LEVELGYROROLL].integratedError = 0;
|
157 |
|
158 |
PID[LEVELGYROPITCH].P = readFloat(LEVEL_GYRO_PITCH_PGAIN_ADR); |
159 |
PID[LEVELGYROPITCH].I = readFloat(LEVEL_GYRO_PITCH_IGAIN_ADR); |
160 |
PID[LEVELGYROPITCH].D = readFloat(LEVEL_GYRO_PITCH_DGAIN_ADR); |
161 |
PID[LEVELGYROPITCH].lastPosition = 0;
|
162 |
PID[LEVELGYROPITCH].integratedError = 0;
|
163 |
|
164 |
#ifdef AltitudeHold
|
165 |
PID[ALTITUDE].P = readFloat(ALTITUDE_PGAIN_ADR); |
166 |
PID[ALTITUDE].I = readFloat(ALTITUDE_IGAIN_ADR); |
167 |
PID[ALTITUDE].D = readFloat(ALTITUDE_DGAIN_ADR); |
168 |
PID[ALTITUDE].lastPosition = 0;
|
169 |
PID[ALTITUDE].integratedError = 0;
|
170 |
PID[ZDAMPENING].P = readFloat(ZDAMP_PGAIN_ADR); |
171 |
PID[ZDAMPENING].I = readFloat(ZDAMP_IGAIN_ADR); |
172 |
PID[ZDAMPENING].D = readFloat(ZDAMP_DGAIN_ADR); |
173 |
PID[ZDAMPENING].lastPosition = 0;
|
174 |
PID[ZDAMPENING].integratedError = 0;
|
175 |
minThrottleAdjust = readFloat(ALTITUDE_MIN_THROTTLE_ADR); |
176 |
maxThrottleAdjust = readFloat(ALTITUDE_MAX_THROTTLE_ADR); |
177 |
altitude.setSmoothFactor(readFloat(ALTITUDE_SMOOTH_ADR)); |
178 |
#endif
|
179 |
|
180 |
|
181 |
#ifdef SonarHold
|
182 |
PID[SONAR].P = 25.0; |
183 |
PID[SONAR].I = 0.0; |
184 |
PID[SONAR].D = 0.0; //findtag |
185 |
sonarMinThrottleAdjust = -150.0; |
186 |
sonarMaxThrottleAdjust = 150.0; |
187 |
#endif
|
188 |
|
189 |
|
190 |
windupGuard = readFloat(WINDUPGUARD_ADR); |
191 |
levelLimit = readFloat(LEVELLIMIT_ADR); |
192 |
levelOff = readFloat(LEVELOFF_ADR); |
193 |
timeConstant = readFloat(FILTERTERM_ADR); |
194 |
smoothHeading = readFloat(HEADINGSMOOTH_ADR); |
195 |
aref = readFloat(AREF_ADR); |
196 |
flightMode = readFloat(FLIGHTMODE_ADR); |
197 |
headingHoldConfig = readFloat(HEADINGHOLD_ADR); |
198 |
minAcro = readFloat(MINACRO_ADR); |
199 |
accel.setOneG(readFloat(ACCEL1G_ADR)); |
200 |
} |
201 |
|
202 |
void writeEEPROM(void){ |
203 |
cli(); // Needed so that APM sensor data doesn't overflow
|
204 |
writeFloat(PID[ROLL].P, PGAIN_ADR); |
205 |
writeFloat(PID[ROLL].I, IGAIN_ADR); |
206 |
writeFloat(PID[ROLL].D, DGAIN_ADR); |
207 |
writeFloat(PID[PITCH].P, PITCH_PGAIN_ADR); |
208 |
writeFloat(PID[PITCH].I, PITCH_IGAIN_ADR); |
209 |
writeFloat(PID[PITCH].D, PITCH_DGAIN_ADR); |
210 |
writeFloat(PID[LEVELROLL].P, LEVEL_PGAIN_ADR); |
211 |
writeFloat(PID[LEVELROLL].I, LEVEL_IGAIN_ADR); |
212 |
writeFloat(PID[LEVELROLL].D, LEVEL_DGAIN_ADR); |
213 |
writeFloat(PID[LEVELPITCH].P, LEVEL_PITCH_PGAIN_ADR); |
214 |
writeFloat(PID[LEVELPITCH].I, LEVEL_PITCH_IGAIN_ADR); |
215 |
writeFloat(PID[LEVELPITCH].D, LEVEL_PITCH_DGAIN_ADR); |
216 |
writeFloat(PID[YAW].P, YAW_PGAIN_ADR); |
217 |
writeFloat(PID[YAW].I, YAW_IGAIN_ADR); |
218 |
writeFloat(PID[YAW].D, YAW_DGAIN_ADR); |
219 |
writeFloat(PID[HEADING].P, HEADING_PGAIN_ADR); |
220 |
writeFloat(PID[HEADING].I, HEADING_IGAIN_ADR); |
221 |
writeFloat(PID[HEADING].D, HEADING_DGAIN_ADR); |
222 |
writeFloat(PID[LEVELGYROROLL].P, LEVEL_GYRO_ROLL_PGAIN_ADR); |
223 |
writeFloat(PID[LEVELGYROROLL].I, LEVEL_GYRO_ROLL_IGAIN_ADR); |
224 |
writeFloat(PID[LEVELGYROROLL].D, LEVEL_GYRO_ROLL_DGAIN_ADR); |
225 |
writeFloat(PID[LEVELGYROPITCH].P, LEVEL_GYRO_PITCH_PGAIN_ADR); |
226 |
writeFloat(PID[LEVELGYROPITCH].I, LEVEL_GYRO_PITCH_IGAIN_ADR); |
227 |
writeFloat(PID[LEVELGYROPITCH].D, LEVEL_GYRO_PITCH_DGAIN_ADR); |
228 |
#ifdef AltitudeHold
|
229 |
writeFloat(PID[ALTITUDE].P, ALTITUDE_PGAIN_ADR); |
230 |
writeFloat(PID[ALTITUDE].I, ALTITUDE_IGAIN_ADR); |
231 |
writeFloat(PID[ALTITUDE].D, ALTITUDE_DGAIN_ADR); |
232 |
writeFloat(PID[ZDAMPENING].P, ZDAMP_PGAIN_ADR); |
233 |
writeFloat(PID[ZDAMPENING].I, ZDAMP_IGAIN_ADR); |
234 |
writeFloat(PID[ZDAMPENING].D, ZDAMP_DGAIN_ADR); |
235 |
writeFloat(minThrottleAdjust, ALTITUDE_MIN_THROTTLE_ADR); |
236 |
writeFloat(maxThrottleAdjust, ALTITUDE_MAX_THROTTLE_ADR); |
237 |
writeFloat(altitude.getSmoothFactor(), ALTITUDE_SMOOTH_ADR); |
238 |
#endif
|
239 |
writeFloat(windupGuard, WINDUPGUARD_ADR); |
240 |
writeFloat(levelLimit, LEVELLIMIT_ADR); |
241 |
writeFloat(levelOff, LEVELOFF_ADR); |
242 |
writeFloat(receiver.getXmitFactor(), XMITFACTOR_ADR); |
243 |
writeFloat(gyro.getSmoothFactor(), GYROSMOOTH_ADR); |
244 |
writeFloat(accel.getSmoothFactor(), ACCSMOOTH_ADR); |
245 |
writeFloat(receiver.getSmoothFactor(THROTTLE), THROTTLESMOOTH_ADR); |
246 |
writeFloat(receiver.getSmoothFactor(ROLL), ROLLSMOOTH_ADR); |
247 |
writeFloat(receiver.getSmoothFactor(PITCH), PITCHSMOOTH_ADR); |
248 |
writeFloat(receiver.getSmoothFactor(YAW), YAWSMOOTH_ADR); |
249 |
writeFloat(receiver.getSmoothFactor(MODE), MODESMOOTH_ADR); |
250 |
writeFloat(receiver.getSmoothFactor(AUX), AUXSMOOTH_ADR); |
251 |
writeFloat(timeConstant, FILTERTERM_ADR); |
252 |
writeFloat(receiver.getTransmitterSlope(THROTTLE), THROTTLESCALE_ADR); |
253 |
writeFloat(receiver.getTransmitterOffset(THROTTLE), THROTTLEOFFSET_ADR); |
254 |
writeFloat(receiver.getTransmitterSlope(ROLL), ROLLSCALE_ADR); |
255 |
writeFloat(receiver.getTransmitterOffset(ROLL), ROLLOFFSET_ADR); |
256 |
writeFloat(receiver.getTransmitterSlope(PITCH), PITCHSCALE_ADR); |
257 |
writeFloat(receiver.getTransmitterOffset(PITCH), PITCHOFFSET_ADR); |
258 |
writeFloat(receiver.getTransmitterSlope(YAW), YAWSCALE_ADR); |
259 |
writeFloat(receiver.getTransmitterOffset(YAW), YAWOFFSET_ADR); |
260 |
writeFloat(receiver.getTransmitterSlope(MODE), MODESCALE_ADR); |
261 |
writeFloat(receiver.getTransmitterOffset(MODE), MODEOFFSET_ADR); |
262 |
writeFloat(receiver.getTransmitterSlope(AUX), AUXSCALE_ADR); |
263 |
writeFloat(receiver.getTransmitterOffset(AUX), AUXOFFSET_ADR); |
264 |
writeFloat(smoothHeading, HEADINGSMOOTH_ADR); |
265 |
writeFloat(aref, AREF_ADR); |
266 |
writeFloat(flightMode, FLIGHTMODE_ADR); |
267 |
writeFloat(headingHoldConfig, HEADINGHOLD_ADR); |
268 |
writeFloat(minAcro, MINACRO_ADR); |
269 |
writeFloat(accel.getOneG(), ACCEL1G_ADR); |
270 |
sei(); // Restart interrupts
|
271 |
} |