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 : #include "AP_Baro_LPS2XH.h"
16 :
17 : #if AP_BARO_LPS2XH_ENABLED
18 :
19 : #include <stdio.h>
20 :
21 : #include <AP_InertialSensor/AP_InertialSensor_Invensense_registers.h>
22 :
23 : extern const AP_HAL::HAL &hal;
24 :
25 : // WHOAMI values
26 : #define LPS22HB_WHOAMI 0xB1
27 : #define LPS25HB_WHOAMI 0xBD
28 :
29 : #define REG_ID 0x0F
30 :
31 : #define LPS22H_ID 0xB1
32 : #define LPS22H_CTRL_REG1 0x10
33 : #define LPS22H_CTRL_REG2 0x11
34 : #define LPS22H_CTRL_REG3 0x12
35 :
36 : #define LPS22H_CTRL_REG1_SIM (1 << 0)
37 : #define LPS22H_CTRL_REG1_BDU (1 << 1)
38 : #define LPS22H_CTRL_REG1_LPFP_CFG (1 << 2)
39 : #define LPS22H_CTRL_REG1_EN_LPFP (1 << 3)
40 : #define LPS22H_CTRL_REG1_PD (0 << 4)
41 : #define LPS22H_CTRL_REG1_ODR_1H (1 << 4)
42 : #define LPS22H_CTRL_REG1_ODR_10HZ (2 << 4)
43 : #define LPS22H_CTRL_REG1_ODR_25HZ (3 << 4)
44 : #define LPS22H_CTRL_REG1_ODR_50HZ (4 << 4)
45 : #define LPS22H_CTRL_REG1_ODR_75HZ (5 << 4)
46 :
47 : #define LPS25H_CTRL_REG1_ADDR 0x20
48 : #define LPS25H_CTRL_REG2_ADDR 0x21
49 : #define LPS25H_CTRL_REG3_ADDR 0x22
50 : #define LPS25H_CTRL_REG4_ADDR 0x23
51 : #define LPS25H_FIFO_CTRL 0x2E
52 : #define TEMP_OUT_ADDR 0x2B
53 : #define PRESS_OUT_XL_ADDR 0x28
54 : #define STATUS_ADDR 0x27
55 :
56 : //putting 1 in the MSB of those two registers turns on Auto increment for faster reading.
57 :
58 0 : AP_Baro_LPS2XH::AP_Baro_LPS2XH(AP_Baro &baro, AP_HAL::Device &dev)
59 : : AP_Baro_Backend(baro)
60 0 : , _dev(&dev)
61 : {
62 0 : }
63 :
64 0 : AP_Baro_Backend *AP_Baro_LPS2XH::probe(AP_Baro &baro, AP_HAL::Device &dev)
65 : {
66 0 : AP_Baro_LPS2XH *sensor = NEW_NOTHROW AP_Baro_LPS2XH(baro, dev);
67 0 : if (!sensor || !sensor->_init()) {
68 0 : delete sensor;
69 0 : return nullptr;
70 : }
71 :
72 0 : return sensor;
73 : }
74 :
75 0 : AP_Baro_Backend *AP_Baro_LPS2XH::probe_InvensenseIMU(AP_Baro &baro,
76 : AP_HAL::Device &dev,
77 : uint8_t imu_address)
78 : {
79 0 : AP_Baro_LPS2XH *sensor = NEW_NOTHROW AP_Baro_LPS2XH(baro, dev);
80 0 : if (sensor) {
81 0 : if (!sensor->_imu_i2c_init(imu_address)) {
82 0 : delete sensor;
83 0 : return nullptr;
84 : }
85 : }
86 :
87 0 : if (!sensor || !sensor->_init()) {
88 0 : delete sensor;
89 0 : return nullptr;
90 : }
91 :
92 0 : return sensor;
93 : }
94 :
95 : /*
96 : setup invensense IMU to enable barometer, assuming both IMU and baro
97 : on the same i2c bus
98 : */
99 0 : bool AP_Baro_LPS2XH::_imu_i2c_init(uint8_t imu_address)
100 : {
101 0 : _dev->get_semaphore()->take_blocking();
102 :
103 : // as the baro device is already locked we need to re-use it,
104 : // changing its address to match the IMU address
105 0 : uint8_t old_address = _dev->get_bus_address();
106 0 : _dev->set_address(imu_address);
107 :
108 0 : _dev->set_retries(4);
109 :
110 0 : uint8_t whoami=0;
111 0 : _dev->read_registers(MPUREG_WHOAMI, &whoami, 1);
112 0 : DEV_PRINTF("IMU: whoami 0x%02x old_address=%02x\n", whoami, old_address);
113 :
114 0 : _dev->write_register(MPUREG_FIFO_EN, 0x00);
115 0 : _dev->write_register(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_CLK_XGYRO);
116 :
117 : // wait for sensor to settle
118 0 : hal.scheduler->delay(10);
119 :
120 0 : _dev->write_register(MPUREG_INT_PIN_CFG, BIT_BYPASS_EN);
121 :
122 0 : _dev->set_address(old_address);
123 :
124 0 : _dev->get_semaphore()->give();
125 :
126 0 : return true;
127 : }
128 :
129 0 : bool AP_Baro_LPS2XH::_init()
130 : {
131 0 : if (!_dev) {
132 0 : return false;
133 : }
134 0 : _dev->get_semaphore()->take_blocking();
135 :
136 0 : _dev->set_speed(AP_HAL::Device::SPEED_HIGH);
137 :
138 : // top bit is for read on SPI
139 0 : if (_dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) {
140 0 : _dev->set_read_flag(0x80);
141 : }
142 :
143 0 : if (!_check_whoami()) {
144 0 : _dev->get_semaphore()->give();
145 0 : return false;
146 : }
147 :
148 : //init control registers.
149 0 : if (_lps2xh_type == BARO_LPS25H) {
150 0 : _dev->write_register(LPS25H_CTRL_REG1_ADDR,0x00); // turn off for config
151 0 : _dev->write_register(LPS25H_CTRL_REG2_ADDR,0x00); //FIFO Disabled
152 0 : _dev->write_register(LPS25H_FIFO_CTRL, 0x01);
153 0 : _dev->write_register(LPS25H_CTRL_REG1_ADDR,0xc0);
154 :
155 : // request 25Hz update (maximum refresh Rate according to datasheet)
156 0 : CallTime = 40 * AP_USEC_PER_MSEC;
157 : }
158 :
159 0 : if (_lps2xh_type == BARO_LPS22H) {
160 0 : _dev->write_register(LPS22H_CTRL_REG1, 0x00); // turn off for config
161 0 : _dev->write_register(LPS22H_CTRL_REG1, LPS22H_CTRL_REG1_ODR_75HZ|LPS22H_CTRL_REG1_BDU|LPS22H_CTRL_REG1_EN_LPFP|LPS22H_CTRL_REG1_LPFP_CFG);
162 0 : if (_dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) {
163 0 : _dev->write_register(LPS22H_CTRL_REG2, 0x18); // disable i2c
164 : } else {
165 0 : _dev->write_register(LPS22H_CTRL_REG2, 0x10);
166 : }
167 :
168 : // request 75Hz update
169 0 : CallTime = 1000000/75;
170 : }
171 :
172 0 : _instance = _frontend.register_sensor();
173 :
174 0 : _dev->set_device_type(DEVTYPE_BARO_LPS2XH);
175 0 : set_bus_id(_instance, _dev->get_bus_id());
176 :
177 0 : _dev->get_semaphore()->give();
178 :
179 0 : _dev->register_periodic_callback(CallTime, FUNCTOR_BIND_MEMBER(&AP_Baro_LPS2XH::_timer, void));
180 :
181 0 : return true;
182 : }
183 :
184 : //check ID
185 0 : bool AP_Baro_LPS2XH::_check_whoami(void)
186 : {
187 : uint8_t whoami;
188 0 : if (!_dev->read_registers(REG_ID, &whoami, 1)) {
189 0 : return false;
190 : }
191 0 : DEV_PRINTF("LPS2XH whoami 0x%02x\n", whoami);
192 :
193 0 : switch(whoami){
194 0 : case LPS22HB_WHOAMI:
195 0 : _lps2xh_type = BARO_LPS22H;
196 0 : return true;
197 0 : case LPS25HB_WHOAMI:
198 0 : _lps2xh_type = BARO_LPS25H;
199 0 : return true;
200 : }
201 :
202 0 : return false;
203 : }
204 :
205 : // accumulate a new sensor reading
206 0 : void AP_Baro_LPS2XH::_timer(void)
207 : {
208 : uint8_t status;
209 : // use status to check if data is available
210 0 : if (!_dev->read_registers(STATUS_ADDR, &status, 1)) {
211 0 : return;
212 : }
213 :
214 0 : if (status & 0x02) {
215 0 : _update_temperature();
216 : }
217 :
218 0 : if (status & 0x01) {
219 0 : _update_pressure();
220 : }
221 : }
222 :
223 : // transfer data to the frontend
224 0 : void AP_Baro_LPS2XH::update(void)
225 : {
226 0 : if (_pressure_count == 0) {
227 0 : return;
228 : }
229 :
230 0 : WITH_SEMAPHORE(_sem);
231 0 : _copy_to_frontend(_instance, _pressure_sum/_pressure_count, _temperature);
232 0 : _pressure_sum = 0;
233 0 : _pressure_count = 0;
234 0 : }
235 :
236 : // calculate temperature
237 0 : void AP_Baro_LPS2XH::_update_temperature(void)
238 : {
239 : uint8_t pu8[2];
240 0 : if (!_dev->read_registers(TEMP_OUT_ADDR, pu8, 2)) {
241 0 : return;
242 : }
243 0 : int16_t Temp_Reg_s16 = (uint16_t)(pu8[1]<<8) | pu8[0];
244 :
245 0 : WITH_SEMAPHORE(_sem);
246 :
247 0 : if (_lps2xh_type == BARO_LPS25H) {
248 0 : _temperature = (Temp_Reg_s16 * (1.0/480)) + 42.5;
249 : }
250 :
251 0 : if (_lps2xh_type == BARO_LPS22H) {
252 0 : _temperature = Temp_Reg_s16 * 0.01;
253 : }
254 0 : }
255 :
256 : // calculate pressure
257 0 : void AP_Baro_LPS2XH::_update_pressure(void)
258 : {
259 : uint8_t pressure[3];
260 0 : if (!_dev->read_registers(PRESS_OUT_XL_ADDR, pressure, 3)) {
261 0 : return;
262 : }
263 :
264 0 : int32_t Pressure_Reg_s32 = ((uint32_t)pressure[2]<<16)|((uint32_t)pressure[1]<<8)|(uint32_t)pressure[0];
265 0 : int32_t Pressure_mb = Pressure_Reg_s32 * (100.0f / 4096); // scale for pa
266 :
267 0 : WITH_SEMAPHORE(_sem);
268 0 : _pressure_sum += Pressure_mb;
269 0 : _pressure_count++;
270 0 : }
271 :
272 : #endif // AP_BARO_LPS2XH_ENABLED
|
