PX4 Firmware
PX4 Autopilot Software http://px4.io
df_bmp280_wrapper.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2016 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file df_bmp280_wrapper.cpp
36  * Lightweight driver to access the BMP280 of the DriverFramework.
37  *
38  * @author Julian Oes <julian@oes.ch>
39  */
40 
41 #include <px4_platform_common/px4_config.h>
42 
43 #include <sys/types.h>
44 #include <sys/stat.h>
45 #include <stdint.h>
46 #include <stddef.h>
47 #include <stdlib.h>
48 #include <string.h>
49 #include <math.h>
50 #include <unistd.h>
51 #include <px4_platform_common/getopt.h>
52 #include <errno.h>
53 
54 #include <perf/perf_counter.h>
55 #include <systemlib/err.h>
56 
57 #include <drivers/drv_hrt.h>
58 #include <drivers/drv_baro.h>
59 
60 #include <board_config.h>
61 //#include <mathlib/math/filter/LowPassFilter2p.hpp>
62 
63 #include <bmp280/BMP280.hpp>
64 #include <DevMgr.hpp>
65 
66 
67 extern "C" { __EXPORT int df_bmp280_wrapper_main(int argc, char *argv[]); }
68 
69 using namespace DriverFramework;
70 
71 
72 class DfBmp280Wrapper : public BMP280
73 {
74 public:
76  ~DfBmp280Wrapper();
77 
78 
79  /**
80  * Start automatic measurement.
81  *
82  * @return 0 on success
83  */
84  int start();
85 
86  /**
87  * Stop automatic measurement.
88  *
89  * @return 0 on success
90  */
91  int stop();
92 
93 private:
94  int _publish(struct baro_sensor_data &data);
95 
97 
99 
101 
102 };
103 
105  BMP280(BARO_DEVICE_PATH),
106  _baro_topic(nullptr),
107  _baro_orb_class_instance(-1),
108  _baro_sample_perf(perf_alloc(PC_ELAPSED, "df_baro_read"))
109 {
110 }
111 
113 {
115 }
116 
118 {
119  /* Init device and start sensor. */
120  int ret = init();
121 
122  if (ret != 0) {
123  PX4_ERR("BMP280 init fail: %d", ret);
124  return ret;
125  }
126 
127  ret = BMP280::start();
128 
129  if (ret != 0) {
130  PX4_ERR("BMP280 start fail: %d", ret);
131  return ret;
132  }
133 
134 #if defined(__DF_BBBLUE)
135  PX4_INFO("BMP280 started");
136 #endif
137 
138  return 0;
139 }
140 
142 {
143  /* Stop sensor. */
144  int ret = BMP280::stop();
145 
146  if (ret != 0) {
147  PX4_ERR("BMP280 stop fail: %d", ret);
148  return ret;
149  }
150 
151  return 0;
152 }
153 
154 int DfBmp280Wrapper::_publish(struct baro_sensor_data &data)
155 {
157 
158  sensor_baro_s baro_report = {};
159  baro_report.timestamp = hrt_absolute_time();
160  baro_report.error_count = data.error_counter;
161 
162  baro_report.pressure = data.pressure_pa / 100.0f; // to mbar
163  baro_report.temperature = data.temperature_c;
164 
165  // TODO: when is this ever blocked?
166  if (!(m_pub_blocked)) {
167 
168  if (_baro_topic == nullptr) {
169  _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &baro_report,
171 
172  } else {
173  orb_publish(ORB_ID(sensor_baro), _baro_topic, &baro_report);
174  }
175  }
176 
178 
179  return 0;
180 };
181 
182 
184 {
185 
187 
188 int start(/* enum Rotation rotation */);
189 int stop();
190 int info();
191 void usage();
192 
193 int start(/*enum Rotation rotation*/)
194 {
195  g_dev = new DfBmp280Wrapper(/*rotation*/);
196 
197  if (g_dev == nullptr) {
198  PX4_ERR("failed instantiating DfBmp280Wrapper object");
199  return -1;
200  }
201 
202  int ret = g_dev->start();
203 
204  if (ret != 0) {
205  PX4_ERR("DfBmp280Wrapper start failed");
206  return ret;
207  }
208 
209  // Open the IMU sensor
210  DevHandle h;
211  DevMgr::getHandle(BARO_DEVICE_PATH, h);
212 
213  if (!h.isValid()) {
214  DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
215  BARO_DEVICE_PATH, h.getError());
216  return -1;
217  }
218 
219  DevMgr::releaseHandle(h);
220 
221  return 0;
222 }
223 
224 int stop()
225 {
226  if (g_dev == nullptr) {
227  PX4_ERR("driver not running");
228  return 1;
229  }
230 
231  int ret = g_dev->stop();
232 
233  if (ret != 0) {
234  PX4_ERR("driver could not be stopped");
235  return ret;
236  }
237 
238  delete g_dev;
239  g_dev = nullptr;
240  return 0;
241 }
242 
243 /**
244  * Print a little info about the driver.
245  */
246 int
248 {
249  if (g_dev == nullptr) {
250  PX4_ERR("driver not running");
251  return 1;
252  }
253 
254  PX4_DEBUG("state @ %p", g_dev);
255 
256  return 0;
257 }
258 
259 void
261 {
262  PX4_WARN("Usage: df_bmp280_wrapper 'start', 'info', 'stop'");
263 }
264 
265 } // namespace df_bmp280_wrapper
266 
267 
268 int
269 df_bmp280_wrapper_main(int argc, char *argv[])
270 {
271  int ret = 0;
272  int myoptind = 1;
273 
274  if (argc <= 1) {
276  return 1;
277  }
278 
279  const char *verb = argv[myoptind];
280 
281 
282  if (!strcmp(verb, "start")) {
283  ret = df_bmp280_wrapper::start();
284  }
285 
286  else if (!strcmp(verb, "stop")) {
287  ret = df_bmp280_wrapper::stop();
288  }
289 
290  else if (!strcmp(verb, "info")) {
291  ret = df_bmp280_wrapper::info();
292  }
293 
294  else {
296  return 1;
297  }
298 
299  return ret;
300 }
measure the time elapsed performing an event
Definition: perf_counter.h:56
void usage(const char *reason)
Print the correct usage.
Definition: Commander.cpp:4238
Definition: I2C.hpp:51
uint64_t error_count
Definition: sensor_baro.h:54
static void stop()
Definition: dataman.cpp:1491
int _publish(struct baro_sensor_data &data)
High-resolution timer with callouts and timekeeping.
void usage()
Prints info about the driver argument usage.
float temperature
Definition: sensor_baro.h:57
__EXPORT int df_bmp280_wrapper_main(int argc, char *argv[])
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
int init()
Definition: BMP280.cpp:60
float pressure
Definition: sensor_baro.h:56
Header common to all counters.
void perf_free(perf_counter_t handle)
Free a counter.
#define perf_alloc(a, b)
Definition: px4io.h:59
uint8_t * data
Definition: dataman.cpp:149
int start()
Start automatic measurement.
Simple error/warning functions, heavily inspired by the BSD functions of the same names...
void perf_end(perf_counter_t handle)
End a performance event.
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
Definition: uORB.cpp:70
orb_advert_t _baro_topic
static int start()
Definition: dataman.cpp:1452
int info()
Print a little info about the driver.
int stop()
Stop the driver.
int stop()
Stop automatic measurement.
int start()
Attempt to start driver on all available I2C busses.
DfBmp280Wrapper * g_dev
uint64_t timestamp
Definition: sensor_baro.h:53
Barometric pressure sensor driver interface.
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority)
Definition: uORB.cpp:53
void perf_begin(perf_counter_t handle)
Begin a performance event.
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
perf_counter_t _baro_sample_perf
Performance measuring tools.