PX4 Firmware
PX4 Autopilot Software http://px4.io
df_ms5607_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_ms5607_wrapper.cpp
36  * Lightweight driver to access the MS5607 of the DriverFramework.
37  */
38 
39 #include <px4_platform_common/px4_config.h>
40 
41 #include <sys/types.h>
42 #include <sys/stat.h>
43 #include <stdint.h>
44 #include <stddef.h>
45 #include <stdlib.h>
46 #include <string.h>
47 #include <math.h>
48 #include <unistd.h>
49 #include <px4_platform_common/getopt.h>
50 #include <errno.h>
51 
52 #include <perf/perf_counter.h>
53 #include <systemlib/err.h>
54 
55 #include <drivers/drv_baro.h>
56 #include <drivers/drv_hrt.h>
57 
58 #include <board_config.h>
59 
60 #include <ms5607/MS5607.hpp>
61 #include <DevMgr.hpp>
62 
63 
64 extern "C" { __EXPORT int df_ms5607_wrapper_main(int argc, char *argv[]); }
65 
66 using namespace DriverFramework;
67 
68 
69 class DfMS5607Wrapper : public MS5607
70 {
71 public:
73  ~DfMS5607Wrapper();
74 
75 
76  /**
77  * Start automatic measurement.
78  *
79  * @return 0 on success
80  */
81  int start();
82 
83  /**
84  * Stop automatic measurement.
85  *
86  * @return 0 on success
87  */
88  int stop();
89 
90 private:
91  int _publish(struct baro_sensor_data &data);
92 
94 
96 
98 
99 };
100 
102  MS5607(BARO_DEVICE_PATH),
103  _baro_topic(nullptr),
104  _baro_orb_class_instance(-1),
105  _baro_sample_perf(perf_alloc(PC_ELAPSED, "df_baro_read"))
106 {
107 }
108 
110 {
112 }
113 
115 {
116  /* Init device and start sensor. */
117  int ret = init();
118 
119  if (ret != 0) {
120  PX4_ERR("MS5607 init fail: %d", ret);
121  return ret;
122  }
123 
124  ret = MS5607::start();
125 
126  if (ret != 0) {
127  PX4_ERR("MS5607 start fail: %d", ret);
128  return ret;
129  }
130 
131  return 0;
132 }
133 
135 {
136  /* Stop sensor. */
137  int ret = MS5607::stop();
138 
139  if (ret != 0) {
140  PX4_ERR("MS5607 stop fail: %d", ret);
141  return ret;
142  }
143 
144  return 0;
145 }
146 
147 int DfMS5607Wrapper::_publish(struct baro_sensor_data &data)
148 {
150 
151  sensor_baro_s baro_report{};
152  baro_report.timestamp = hrt_absolute_time();
153 
154  baro_report.pressure = data.pressure_pa / 100.0f; // convert to mbar
155  baro_report.temperature = data.temperature_c;
156  baro_report.error_count = data.error_counter;
157  baro_report.device_id = m_id.dev_id;
158 
159  // TODO: when is this ever blocked?
160  if (!(m_pub_blocked)) {
161 
162  if (_baro_topic == nullptr) {
163  _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &baro_report,
165 
166  } else {
167  orb_publish(ORB_ID(sensor_baro), _baro_topic, &baro_report);
168  }
169  }
170 
172 
173  return 0;
174 };
175 
176 
178 {
179 
181 
182 int start(/* enum Rotation rotation */);
183 int stop();
184 int info();
185 void usage();
186 
187 int start(/*enum Rotation rotation*/)
188 {
189  g_dev = new DfMS5607Wrapper(/*rotation*/);
190 
191  if (g_dev == nullptr) {
192  PX4_ERR("failed instantiating DfMS5607Wrapper object");
193  return -1;
194  }
195 
196  int ret = g_dev->start();
197 
198  if (ret != 0) {
199  PX4_ERR("DfMS5607Wrapper start failed");
200  return ret;
201  }
202 
203  // Open the IMU sensor
204  DevHandle h;
205  DevMgr::getHandle(BARO_DEVICE_PATH, h);
206 
207  if (!h.isValid()) {
208  DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
209  BARO_DEVICE_PATH, h.getError());
210  return -1;
211  }
212 
213  DevMgr::releaseHandle(h);
214 
215  return 0;
216 }
217 
218 int stop()
219 {
220  if (g_dev == nullptr) {
221  PX4_ERR("driver not running");
222  return 1;
223  }
224 
225  int ret = g_dev->stop();
226 
227  if (ret != 0) {
228  PX4_ERR("driver could not be stopped");
229  return ret;
230  }
231 
232  delete g_dev;
233  g_dev = nullptr;
234  return 0;
235 }
236 
237 /**
238  * Print a little info about the driver.
239  */
240 int
242 {
243  if (g_dev == nullptr) {
244  PX4_ERR("driver not running");
245  return 1;
246  }
247 
248  PX4_DEBUG("state @ %p", g_dev);
249 
250  return 0;
251 }
252 
253 void
255 {
256  PX4_WARN("Usage: df_ms5607_wrapper 'start', 'info', 'stop'");
257 }
258 
259 } // namespace df_ms5607_wrapper
260 
261 
262 int
263 df_ms5607_wrapper_main(int argc, char *argv[])
264 {
265  int ret = 0;
266  int myoptind = 1;
267 
268  if (argc <= 1) {
270  return 1;
271  }
272 
273  const char *verb = argv[myoptind];
274 
275 
276  if (!strcmp(verb, "start")) {
277  ret = df_ms5607_wrapper::start();
278  }
279 
280  else if (!strcmp(verb, "stop")) {
281  ret = df_ms5607_wrapper::stop();
282  }
283 
284  else if (!strcmp(verb, "info")) {
285  ret = df_ms5607_wrapper::info();
286  }
287 
288  else {
290  return 1;
291  }
292 
293  return ret;
294 }
int info()
Print a little info about the driver.
orb_advert_t _baro_topic
measure the time elapsed performing an event
Definition: perf_counter.h:56
int start()
Start automatic measurement.
perf_counter_t _baro_sample_perf
void usage(const char *reason)
Print the correct usage.
Definition: Commander.cpp:4238
int stop()
Stop automatic measurement.
Definition: I2C.hpp:51
static void stop()
Definition: dataman.cpp:1491
High-resolution timer with callouts and timekeeping.
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
int _publish(struct baro_sensor_data &data)
Header common to all counters.
void perf_free(perf_counter_t handle)
Free a counter.
void init()
Activates/configures the hardware registers.
#define perf_alloc(a, b)
Definition: px4io.h:59
uint8_t * data
Definition: dataman.cpp:149
DfMS5607Wrapper * g_dev
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
static int start()
Definition: dataman.cpp:1452
uint64_t timestamp
Definition: sensor_baro.h:53
__EXPORT int df_ms5607_wrapper_main(int argc, char *argv[])
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
int stop()
Stop the driver.
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).
int start()
Attempt to start driver on all available I2C busses.
void usage()
Prints info about the driver argument usage.
Performance measuring tools.