Statistics
| Branch: | Revision:

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
}