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
|
/*
* Copyright © 2009 Keith Packard <keithp@keithp.com>
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; version 2 of the License.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
*/
#include "ao.h"
/* Main flight thread. */
__xdata struct ao_adc ao_flight_data; /* last acquired data */
__pdata enum flight_state ao_flight_state; /* current flight state */
__pdata uint16_t ao_flight_tick; /* time of last data */
__pdata int16_t ao_flight_accel; /* filtered acceleration */
__pdata int16_t ao_flight_pres; /* filtered pressure */
__pdata int16_t ao_ground_pres; /* startup pressure */
__pdata int16_t ao_ground_accel; /* startup acceleration */
__pdata int16_t ao_min_pres; /* minimum recorded pressure */
__pdata uint16_t ao_launch_time; /* time of launch detect */
__pdata int16_t ao_main_pres; /* pressure to eject main */
/*
* track min/max data over a long interval to detect
* resting
*/
__pdata uint16_t ao_interval_end;
__pdata int16_t ao_interval_cur_min_accel;
__pdata int16_t ao_interval_cur_max_accel;
__pdata int16_t ao_interval_cur_min_pres;
__pdata int16_t ao_interval_cur_max_pres;
__pdata int16_t ao_interval_min_accel;
__pdata int16_t ao_interval_max_accel;
__pdata int16_t ao_interval_min_pres;
__pdata int16_t ao_interval_max_pres;
#define AO_INTERVAL_TICKS AO_SEC_TO_TICKS(5)
/* Accelerometer calibration
*
* We're sampling the accelerometer through a resistor divider which
* consists of 5k and 10k resistors. This multiplies the values by 2/3.
* That goes into the cc1111 A/D converter, which is running at 11 bits
* of precision with the bits in the MSB of the 16 bit value. Only positive
* values are used, so values should range from 0-32752 for 0-3.3V. The
* specs say we should see 40mV/g (uncalibrated), multiply by 2/3 for what
* the A/D converter sees (26.67 mV/g). We should see 32752/3300 counts/mV,
* for a final computation of:
*
* 26.67 mV/g * 32767/3300 counts/mV = 264.8 counts/g
*
* Zero g was measured at 16000 (we would expect 16384)
*/
#define ACCEL_G 265
#define ACCEL_ZERO_G 16000
#define ACCEL_NOSE_UP (ACCEL_ZERO_G - ACCEL_G * 2 /3)
#define ACCEL_BOOST (ACCEL_NOSE_UP - ACCEL_G * 2)
/*
* Barometer calibration
*
* We directly sample the barometer. The specs say:
*
* Pressure range: 15-115 kPa
* Voltage at 115kPa: 2.82
* Output scale: 27mV/kPa
*
* If we want to detect launch with the barometer, we need
* a large enough bump to not be fooled by noise. At typical
* launch elevations (0-2000m), a 200Pa pressure change cooresponds
* to about a 20m elevation change. This is 5.4mV, or about 3LSB.
* As all of our calculations are done in 16 bits, we'll actually see a change
* of 16 times this though
*
* 27 mV/kPa * 32767 / 3300 counts/mV = 268.1 counts/kPa
*/
#define BARO_kPa 268
#define BARO_LAUNCH (BARO_kPa / 5) /* .2kPa */
#define BARO_APOGEE (BARO_kPa / 10) /* .1kPa */
#define BARO_MAIN (BARO_kPa) /* 1kPa */
#define BARO_LAND (BARO_kPa / 20) /* .05kPa */
/* We also have a clock, which can be used to sanity check things in
* case of other failures
*/
#define BOOST_TICKS_MAX AO_SEC_TO_TICKS(10)
void
ao_flight(void)
{
__pdata static uint8_t nsamples = 0;
for (;;) {
ao_sleep(&ao_adc_ring);
ao_adc_get(&ao_flight_data);
ao_flight_accel -= ao_flight_accel >> 4;
ao_flight_accel += ao_flight_data.accel >> 4;
ao_flight_pres -= ao_flight_pres >> 4;
ao_flight_pres += ao_flight_data.pres >> 4;
ao_flight_tick = ao_time();
ao_flight_tick = ao_time();
if ((int16_t) (ao_flight_tick - ao_interval_end) >= 0) {
ao_interval_max_pres = ao_interval_cur_max_pres;
ao_interval_min_pres = ao_interval_cur_min_pres;
ao_interval_max_accel = ao_interval_cur_max_accel;
ao_interval_min_accel = ao_interval_cur_min_accel;
ao_interval_end = ao_flight_tick + AO_INTERVAL_TICKS;
}
switch (ao_flight_state) {
case ao_flight_startup:
if (nsamples < 100) {
++nsamples;
continue;
}
ao_ground_accel = ao_flight_accel;
ao_ground_pres = ao_flight_pres;
ao_min_pres = ao_flight_pres;
ao_main_pres = ao_ground_pres - BARO_MAIN;
ao_interval_end = ao_flight_tick;
if (ao_flight_accel < ACCEL_NOSE_UP) {
ao_flight_state = ao_flight_launchpad;
ao_report_notify();
} else {
ao_flight_state = ao_flight_idle;
ao_report_notify();
}
/* signal successful initialization by turning off the LED */
ao_led_off(AO_LED_RED);
break;
case ao_flight_launchpad:
if (ao_flight_accel < ACCEL_BOOST ||
ao_flight_pres + BARO_LAUNCH < ao_ground_pres)
{
ao_flight_state = ao_flight_boost;
ao_log_start();
ao_report_notify();
break;
}
break;
case ao_flight_boost:
if (ao_flight_accel > ACCEL_ZERO_G ||
(int16_t) (ao_flight_data.tick - ao_launch_time) > BOOST_TICKS_MAX)
{
ao_flight_state = ao_flight_coast;
ao_report_notify();
break;
}
break;
case ao_flight_coast:
if (ao_flight_pres < ao_min_pres)
ao_min_pres = ao_flight_pres;
if (ao_flight_pres - BARO_APOGEE > ao_min_pres) {
ao_flight_state = ao_flight_apogee;
ao_report_notify();
}
break;
case ao_flight_apogee:
// ao_ignite(AO_IGNITE_DROGUE);
ao_flight_state = ao_flight_drogue;
ao_report_notify();
break;
case ao_flight_drogue:
if (ao_flight_pres >= ao_main_pres) {
// ao_ignite(AO_IGNITE_MAIN);
ao_flight_state = ao_flight_main;
ao_report_notify();
}
if ((ao_interval_max_pres - ao_interval_min_pres) < BARO_LAND) {
ao_flight_state = ao_flight_landed;
ao_report_notify();
}
break;
case ao_flight_main:
if ((ao_interval_max_pres - ao_interval_min_pres) < BARO_LAND) {
ao_flight_state = ao_flight_landed;
ao_report_notify();
}
break;
case ao_flight_landed:
ao_log_stop();
break;
}
}
}
static __xdata struct ao_task flight_task;
void
ao_flight_init(void)
{
ao_flight_state = ao_flight_startup;
ao_interval_min_accel = 0;
ao_interval_max_accel = 0x7fff;
ao_interval_min_pres = 0;
ao_interval_max_pres = 0x7fff;
ao_interval_end = AO_INTERVAL_TICKS;
ao_add_task(&flight_task, ao_flight, "flight");
}
|
