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 rewriting 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 
} 