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
|
/*
* 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; either version 2 of the License, or
* (at your option) any later version.
*
* 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;
__data enum flight_state ao_flight_state;
__data uint16_t ao_flight_state_tick;
__data int16_t ao_flight_accel;
__data int16_t ao_flight_pres;
__data int16_t ao_ground_pres;
__data int16_t ao_ground_accel;
__data int16_t ao_min_pres;
__data uint16_t ao_launch_time;
/* 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 */
/* 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)
{
__data 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;
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;
if (ao_flight_accel < ACCEL_NOSE_UP) {
ao_flight_state = ao_flight_launchpad;
ao_flight_state_tick = ao_time();
ao_report_notify();
} else {
ao_flight_state = ao_flight_idle;
ao_flight_state_tick = ao_time();
ao_report_notify();
}
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_flight_state_tick = ao_time();
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_flight_state_tick = ao_time();
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_flight_state_tick = ao_time();
ao_report_notify();
}
break;
case ao_flight_apogee:
break;
}
}
}
static __xdata struct ao_task flight_task;
void
ao_flight_init(void)
{
ao_flight_state = ao_flight_startup;
ao_add_task(&flight_task, ao_flight);
}
|
