PX4 Firmware
PX4 Autopilot Software http://px4.io
PX4Rangefinder.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2019 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 #include "PX4Rangefinder.hpp"
35 
37 
38 PX4Rangefinder::PX4Rangefinder(const uint32_t device_id, const uint8_t priority, const uint8_t device_orientation) :
39  CDev(nullptr),
40  _distance_sensor_pub{ORB_ID(distance_sensor), priority}
41 {
43 
45  set_orientation(device_orientation);
46 }
47 
49 {
50  if (_class_device_instance != -1) {
52  }
53 }
54 
55 void
56 PX4Rangefinder::set_device_type(uint8_t device_type)
57 {
58  // TODO: range finders should have device ids
59 
60  // // current DeviceStructure
61  // union device::Device::DeviceId device_id;
62  // device_id.devid = _distance_sensor_pub.get().device_id;
63 
64  // // update to new device type
65  // device_id.devid_s.devtype = devtype;
66 
67  // // copy back to report
68  // _distance_sensor_pub.get().device_id = device_id.devid;
69 }
70 
71 void
72 PX4Rangefinder::set_orientation(const uint8_t device_orientation)
73 {
74  _distance_sensor_pub.get().orientation = device_orientation;
75 }
76 
77 void
78 PX4Rangefinder::update(const hrt_abstime timestamp, const float distance, const int8_t quality)
79 {
81 
82  report.timestamp = timestamp;
83  report.current_distance = distance;
84  report.signal_quality = quality;
85 
86  // if quality is unavailable (-1) set to 0 if distance is outside bounds
87  if (quality < 0) {
88  if ((distance < report.min_distance) || (distance > report.max_distance)) {
89  report.signal_quality = 0;
90  }
91  }
92 
94 }
95 
96 void
98 {
99  PX4_INFO(RANGE_FINDER_BASE_DEVICE_PATH " device instance: %d", _class_device_instance);
100 
101  print_message(_distance_sensor_pub.get());
102 }
~PX4Rangefinder() override
#define RANGE_FINDER_BASE_DEVICE_PATH
virtual int register_class_devname(const char *class_devname)
Register a class device name, automatically adding device class instance suffix if need be...
Definition: CDev.cpp:78
static int32_t device_id[max_accel_sens]
void set_device_type(uint8_t device_type)
uORB::PublicationMultiData< distance_sensor_s > _distance_sensor_pub
Definitions for the generic base classes in the device framework.
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
void update(const hrt_abstime timestamp, const float distance, const int8_t quality=-1)
virtual int unregister_class_devname(const char *class_devname, unsigned class_instance)
Register a class device name, automatically adding device class instance suffix if need be...
Definition: CDev.cpp:109
PX4Rangefinder(const uint32_t device_id, const uint8_t priority=ORB_PRIO_DEFAULT, const uint8_t device_orientation=distance_sensor_s::ROTATION_DOWNWARD_FACING)
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
void set_device_id(const uint8_t device_id)
void set_orientation(const uint8_t device_orientation=distance_sensor_s::ROTATION_DOWNWARD_FACING)