Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : /*
16 : FCM320 barometer driver
17 : */
18 :
19 : #include "AP_Baro_FBM320.h"
20 :
21 : #if AP_BARO_FBM320_ENABLED
22 :
23 : #include <stdio.h>
24 : #include <AP_Math/definitions.h>
25 :
26 : extern const AP_HAL::HAL &hal;
27 :
28 : #define FBM320_REG_ID 0x6B
29 : #define FBM320_REG_DATA 0xF6
30 : #define FBM320_REG_CMD 0xF4
31 :
32 : #define FBM320_CMD_READ_T 0x2E
33 : #define FBM320_CMD_READ_P 0xF4
34 :
35 : #define FBM320_WHOAMI 0x42
36 :
37 0 : AP_Baro_FBM320::AP_Baro_FBM320(AP_Baro &baro, AP_HAL::Device &_dev)
38 : : AP_Baro_Backend(baro)
39 0 : , dev(&_dev)
40 : {
41 0 : }
42 :
43 0 : AP_Baro_Backend *AP_Baro_FBM320::probe(AP_Baro &baro, AP_HAL::Device &_dev)
44 : {
45 0 : AP_Baro_FBM320 *sensor = NEW_NOTHROW AP_Baro_FBM320(baro, _dev);
46 0 : if (!sensor || !sensor->init()) {
47 0 : delete sensor;
48 0 : return nullptr;
49 : }
50 0 : return sensor;
51 : }
52 :
53 : /*
54 : read calibration data
55 : */
56 0 : bool AP_Baro_FBM320::read_calibration(void)
57 : {
58 : uint8_t tmp[2];
59 : uint16_t R[10];
60 :
61 0 : for (uint8_t i=0; i<9; i++) {
62 0 : if (!dev->read_registers(0xAA+(i*2),&tmp[0],1)) {
63 0 : return false;
64 : }
65 0 : if (!dev->read_registers(0xAB+(i*2),&tmp[1],1)) {
66 0 : return false;
67 : }
68 0 : R[i] = ((uint8_t)tmp[0] << 8 | tmp[1]);
69 : }
70 :
71 0 : if (!dev->read_registers(0xA4,&tmp[0],1)) {
72 0 : return false;
73 : }
74 0 : if (!dev->read_registers(0xF1,&tmp[0],1)) {
75 0 : return false;
76 : }
77 0 : R[9] = ((uint8_t)tmp[0] << 8) | tmp[1];
78 :
79 :
80 : /* Use R0~R9 calculate C0~C12 of FBM320-02 */
81 0 : calibration.C0 = R[0] >> 4;
82 0 : calibration.C1 = ((R[1] & 0xFF00) >> 5) | (R[2] & 7);
83 0 : calibration.C2 = ((R[1] & 0xFF) << 1) | (R[4] & 1);
84 0 : calibration.C3 = R[2] >> 3;
85 0 : calibration.C4 = ((uint32_t)R[3] << 2) | (R[0] & 3);
86 0 : calibration.C5 = R[4] >> 1;
87 0 : calibration.C6 = R[5] >> 3;
88 0 : calibration.C7 = ((uint32_t)R[6] << 3) | (R[5] & 7);
89 0 : calibration.C8 = R[7] >> 3;
90 0 : calibration.C9 = R[8] >> 2;
91 0 : calibration.C10 = ((R[9] & 0xFF00) >> 6) | (R[8] & 3);
92 0 : calibration.C11 = R[9] & 0xFF;
93 0 : calibration.C12 = ((R[0] & 0x0C) << 1) | (R[7] & 7);
94 :
95 0 : return true;
96 : }
97 :
98 0 : bool AP_Baro_FBM320::init()
99 : {
100 0 : if (!dev) {
101 0 : return false;
102 : }
103 0 : dev->get_semaphore()->take_blocking();
104 :
105 0 : dev->set_speed(AP_HAL::Device::SPEED_HIGH);
106 :
107 : uint8_t whoami;
108 0 : if (!dev->read_registers(FBM320_REG_ID, &whoami, 1) ||
109 0 : whoami != FBM320_WHOAMI) {
110 : // not a FBM320
111 0 : dev->get_semaphore()->give();
112 0 : return false;
113 : }
114 0 : printf("FBM320 ID 0x%x\n", whoami);
115 :
116 0 : if (!read_calibration()) {
117 0 : dev->get_semaphore()->give();
118 0 : return false;
119 : }
120 :
121 0 : dev->write_register(FBM320_REG_CMD, FBM320_CMD_READ_T);
122 :
123 0 : instance = _frontend.register_sensor();
124 :
125 0 : dev->set_device_type(DEVTYPE_BARO_FBM320);
126 0 : set_bus_id(instance, dev->get_bus_id());
127 :
128 0 : dev->get_semaphore()->give();
129 :
130 : // request 50Hz update
131 0 : dev->register_periodic_callback(20 * AP_USEC_PER_MSEC, FUNCTOR_BIND_MEMBER(&AP_Baro_FBM320::timer, void));
132 :
133 0 : return true;
134 : }
135 :
136 : /*
137 : calculate corrected pressure and temperature
138 : */
139 0 : void AP_Baro_FBM320::calculate_PT(int32_t UT, int32_t UP, int32_t &pressure, int32_t &temperature)
140 : {
141 0 : const struct fbm320_calibration &cal = calibration;
142 : int32_t DT, DT2, X01, X02, X03, X11, X12, X13, X21, X22, X23, X24, X25, X26, X31, X32, CF, PP1, PP2, PP3, PP4;
143 :
144 0 : DT = ((UT - 8388608) >> 4) + (cal.C0 << 4);
145 0 : X01 = (cal.C1 + 4459) * DT >> 1;
146 0 : X02 = ((((cal.C2 - 256) * DT) >> 14) * DT) >> 4;
147 0 : X03 = (((((cal.C3 * DT) >> 18) * DT) >> 18) * DT);
148 :
149 0 : temperature = ((2500 << 15) - X01 - X02 - X03) >> 15;
150 :
151 0 : DT2 = (X01 + X02 + X03) >> 12;
152 0 : X11 = ((cal.C5 - 4443) * DT2);
153 0 : X12 = (((cal.C6 * DT2) >> 16) * DT2) >> 2;
154 0 : X13 = ((X11 + X12) >> 10) + ((cal.C4 + 120586) << 4);
155 :
156 0 : X21 = ((cal.C8 + 7180) * DT2) >> 10;
157 0 : X22 = (((cal.C9 * DT2) >> 17) * DT2) >> 12;
158 0 : X23 = (X22 >= X21) ? (X22 - X21) : (X21 - X22);
159 :
160 0 : X24 = (X23 >> 11) * (cal.C7 + 166426);
161 0 : X25 = ((X23 & 0x7FF) * (cal.C7 + 166426)) >> 11;
162 0 : X26 = (X21 >= X22) ? (((0 - X24 - X25) >> 11) + cal.C7 + 166426) : (((X24 + X25) >> 11) + cal.C7 + 166426);
163 :
164 0 : PP1 = ((UP - 8388608) - X13) >> 3;
165 0 : PP2 = (X26 >> 11) * PP1;
166 0 : PP3 = ((X26 & 0x7FF) * PP1) >> 11;
167 0 : PP4 = (PP2 + PP3) >> 10;
168 :
169 0 : CF = (2097152 + cal.C12 * DT2) >> 3;
170 0 : X31 = (((CF * cal.C10) >> 17) * PP4) >> 2;
171 0 : X32 = (((((CF * cal.C11) >> 15) * PP4) >> 18) * PP4);
172 :
173 0 : pressure = ((X31 + X32) >> 15) + PP4 + 99880;
174 0 : }
175 :
176 : // accumulate a new sensor reading
177 0 : void AP_Baro_FBM320::timer(void)
178 : {
179 : uint8_t buf[3];
180 :
181 0 : if (!dev->read_registers(0xF6, buf, sizeof(buf))) {
182 0 : return;
183 : }
184 0 : int32_t value = ((uint32_t)buf[0] << 16) | ((uint32_t)buf[1] << 8) | (uint32_t)buf[2];
185 :
186 0 : if (step == 0) {
187 0 : value_T = value;
188 : } else {
189 : int32_t pressure, temperature;
190 0 : calculate_PT(value_T, value, pressure, temperature);
191 0 : if (pressure_ok(pressure)) {
192 0 : WITH_SEMAPHORE(_sem);
193 0 : pressure_sum += pressure;
194 : // sum and convert to degrees
195 0 : temperature_sum += temperature*0.01;
196 0 : count++;
197 0 : }
198 : }
199 :
200 0 : if (step++ >= 5) {
201 0 : dev->write_register(FBM320_REG_CMD, FBM320_CMD_READ_T);
202 0 : step = 0;
203 : } else {
204 0 : dev->write_register(FBM320_REG_CMD, FBM320_CMD_READ_P);
205 : }
206 : }
207 :
208 : // transfer data to the frontend
209 0 : void AP_Baro_FBM320::update(void)
210 : {
211 0 : if (count == 0) {
212 0 : return;
213 : }
214 0 : WITH_SEMAPHORE(_sem);
215 :
216 0 : _copy_to_frontend(instance, pressure_sum/count, temperature_sum/count);
217 0 : pressure_sum = 0;
218 0 : temperature_sum = 0;
219 0 : count=0;
220 0 : }
221 :
222 : #endif // AP_BARO_FBM320_ENABLED
|
