root / quad1 / AeroQuad / DataStorage.h @ 9240aaa3
History | View | Annotate | Download (9.82 KB)
1 | 9240aaa3 | Alex | /*
|
---|---|---|---|
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 | } |