-
Notifications
You must be signed in to change notification settings - Fork 0
/
AltiUno_attiny85_filter.ino
299 lines (257 loc) · 6.83 KB
/
AltiUno_attiny85_filter.ino
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
/*
Model Rocket single deployment altimeter Ver 1.4
Copyright Boris du Reau 2012-2021
This is using a BMP085 presure sensor and an Attiny 85
The following should fire the main at apogee if it is at least 50m above ground of the launch site
For the BMP085 pressure sensor
Connect VCC of the BMP085 sensor to 5.0V! make sure that you are using the 5V sensor (GY-65 model)
Connect GND to Ground
Connect SCL to i2c clock - pin 7 (PB2) of the ATtiny 85
Connect SDA to i2c data - pin 5 (PB0) of the ATtiny 85
EOC is not used, it signifies an end of conversion
XCLR is a reset pin, also not used here
The apogee is connected to pin 2 (PB3)
The apogee continuity test is connected to pin 3 (PB4)
The speaker/buzzer is connected to pin 6 (PB1)
*/
#include <TinyWireM.h>
#include <tinyBMP085.h>
#define DEBUG //=true
BMP085 bmp;
//ground level altitude
long initialAltitude;
//current altitude
long currAltitude;
//Apogee altitude
long apogeeAltitude;
long liftoffAltitude;
long lastAltitude;
//Our drogue has been ejected i.e: apogee has been detected
boolean apogeeHasFired =false;
//nbr of measures to do so that we are sure that apogee has been reached
unsigned long measures =5;
// those have been changed for the ATtiny 85
const int pinApogee = 3;
const int pinApogeeContinuity = 4;
const int pinSpeaker = 1;
int nbrLongBeep=0;
int nbrShortBeep=0;
boolean NoBeep=false;
//Kalman Variables
float f_1=1.00000; //cast as float
float kalman_x;
float kalman_x_last;
float kalman_p;
float kalman_p_last;
float kalman_k;
float kalman_q;
float kalman_r;
float kalman_x_temp;
float kalman_p_temp;
//float KAlt;
//end of Kalman Variables
void setup()
{
//Initialise the output pin
pinMode(pinApogee, OUTPUT);
pinMode(pinSpeaker, OUTPUT);
pinMode(pinApogeeContinuity, INPUT);
//Make sure that the output are turned off
digitalWrite(pinApogee, LOW);
digitalWrite(pinSpeaker, LOW);
//init Kalman filter
KalmanInit();
//Wire.begin();
TinyWireM.begin();
//Presure Sensor Initialisation
//bmp.begin();
// bmp.begin( BMP085_STANDARD);
//Low res should work better at high speed
bmp.begin( BMP085_ULTRALOWPOWER);
//initialisation give the version of the altimeter
//One long beep per major number and One short beep per minor revision
//For example version 1.2 would be one long beep and 2 short beep
beepAltiVersion(1,4);
//our drogue has not been fired
apogeeHasFired=false;
// let's do some dummy altitude reading
// to initialise the Kalman filter
for (int i=0; i<50; i++){
KalmanCalc(bmp.readAltitude());
}
//let's read the lauch site altitude
long sum = 0;
for (int i=0; i<10; i++){
sum += KalmanCalc(bmp.readAltitude());
delay(50); }
initialAltitude = (sum / 10.0);
lastAltitude = 0;
liftoffAltitude = 20;
//number of measures to do to detect Apogee
measures = 5;
}
void loop()
{
//read current altitude
currAltitude = (KalmanCalc(bmp.readAltitude())- initialAltitude);
if (( currAltitude > liftoffAltitude) != true)
{
continuityCheck(pinApogeeContinuity);
}
//detect apogee
if(currAltitude > liftoffAltitude)
{
if (currAltitude < lastAltitude)
{
measures = measures - 1;
if (measures == 0)
{
//fire drogue
digitalWrite(pinApogee, HIGH);
delay (2000);
apogeeHasFired=true;
digitalWrite(pinApogee, LOW);
//apogeeAltitude = currAltitude;
apogeeAltitude = lastAltitude;
}
}
else
{
lastAltitude = currAltitude;
measures = 5;
}
}
if(apogeeHasFired == true)
{
beepAltitude(apogeeAltitude);
}
}
void continuityCheck(int pin)
{
int val = 0; // variable to store the read value
// read the input pin to check the continuity if apogee has not fired
if (apogeeHasFired == false )
{
val = digitalRead(pin);
if (val == 0)
{
//no continuity long beep
longBeep();
}
else
{
//continuity short beep
shortBeep();
}
}
}
void beepAltitude(long altitude)
{
int i;
// this is the laste thing that I need to write, some code to beep the altitude
//altitude is in meters
//find how many digits
if(altitude > 99)
{
// 1 long beep per hundred meter
nbrLongBeep= int(altitude /100);
//then calculate the number of short beep
nbrShortBeep = (altitude - (nbrLongBeep * 100)) / 10;
}
else
{
nbrLongBeep = 0;
nbrShortBeep = (altitude/10);
}
if (nbrLongBeep > 0)
for (i = 1; i < nbrLongBeep +1 ; i++)
{
longBeep();
delay(50);
}
if (nbrShortBeep > 0)
for (i = 1; i < nbrShortBeep +1 ; i++)
{
shortBeep();
delay(50);
}
delay(5000);
}
void beginBeepSeq()
{
int i=0;
if (NoBeep == false)
{
for (i=0; i<10;i++)
{
tone(pinSpeaker, 1600,1000);
delay(50);
noTone(pinSpeaker);
}
delay(1000);
}
}
void longBeep()
{
if (NoBeep == false)
{
tone(pinSpeaker, 600,1000);
delay(1500);
noTone(pinSpeaker);
}
}
void shortBeep()
{
if (NoBeep == false)
{
tone(pinSpeaker, 600,25);
delay(300);
noTone(pinSpeaker);
}
}
//================================================================
// Kalman functions in your code
//================================================================
//Call KalmanInit() once.
//KalmanInit() - Call before any iterations of KalmanCalc()
void KalmanInit()
{
kalman_q=4.0001; //filter parameters, you can play around with them
kalman_r=.20001; // but these values appear to be fairly optimal
kalman_x = 0;
kalman_p = 0;
kalman_x_temp = 0;
kalman_p_temp = 0;
kalman_x_last = 0;
kalman_p_last = 0;
}
//KalmanCalc() - Calculates new Kalman values from float value "altitude"
// This will be the ASL altitude during the flight, and the AGL altitude during dumps
float KalmanCalc (float altitude)
{
//Predict kalman_x_temp, kalman_p_temp
kalman_x_temp = kalman_x_last;
kalman_p_temp = kalman_p_last + kalman_r;
//Update kalman values
kalman_k = (f_1/(kalman_p_temp + kalman_q)) * kalman_p_temp;
kalman_x = kalman_x_temp + (kalman_k * (altitude - kalman_x_temp));
kalman_p = (f_1 - kalman_k) * kalman_p_temp;
//Save this state for next time
kalman_x_last = kalman_x;
kalman_p_last = kalman_p;
//Assign current Kalman filtered altitude to working variables
//KAlt = kalman_x; //FLOAT Kalman-filtered altitude value
return kalman_x;
}
void beepAltiVersion (int majorNbr, int minorNbr)
{
int i;
for (i=0; i<majorNbr;i++)
{
longBeep();
}
for (i=0; i<minorNbr;i++)
{
shortBeep();
}
}