PX4 Firmware
PX4 Autopilot Software http://px4.io
df_ltc2946_wrapper.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2018 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_ltc2946_wrapper.cpp
36  * Driver to access the LTC2946 of the DriverFramework.
37  *
38  * @author Christoph Tobler <christoph@px4.io>
39  */
40 
41 
42 #include <string>
43 #include <px4_platform_common/px4_config.h>
44 #include <systemlib/err.h>
45 #include <battery/battery.h>
46 #include <drivers/drv_hrt.h>
47 
48 #include <ltc2946/LTC2946.hpp>
49 
50 #include <uORB/uORB.h>
54 
55 
56 extern "C" { __EXPORT int df_ltc2946_wrapper_main(int argc, char *argv[]); }
57 
58 using namespace DriverFramework;
59 
60 
61 class DfLtc2946Wrapper : public LTC2946
62 {
63 public:
66 
67  int start();
68  int stop();
69 
70 private:
71  int _publish(const struct ltc2946_sensor_data &data);
72 
73  orb_advert_t _battery_pub{nullptr};
74  battery_status_s _battery_status{};
75  Battery _battery{};
76 
77  int _actuator_ctrl_0_sub{-1};
78  int _vcontrol_mode_sub{-1};
79 
80 };
81 
83  LTC2946(LTC2946_DEVICE_PATH)
84 {
86 
87  // subscriptions
88  _actuator_ctrl_0_sub = orb_subscribe(ORB_ID(actuator_controls_0));
89  _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
90 }
91 
93 {
96 }
97 
99 {
100  int ret;
101 
102  /* Init device and start sensor. */
103  ret = init();
104 
105  //
106  if (ret != 0) {
107  PX4_ERR("LTC2946 init fail: %d", ret);
108  return ret;
109  }
110 
111  ret = LTC2946::start();
112 
113  if (ret != 0) {
114  PX4_ERR("LTC2946 start fail: %d", ret);
115  return ret;
116  }
117 
118  return 0;
119 }
120 
122 {
123  /* Stop sensor. */
124  int ret = LTC2946::stop();
125 
126  if (ret != 0) {
127  PX4_ERR("LTC2946 stop fail: %d", ret);
128  return ret;
129  }
130 
131  return 0;
132 }
133 
134 int DfLtc2946Wrapper::_publish(const struct ltc2946_sensor_data &data)
135 {
137  bool connected = data.battery_voltage_V > BOARD_ADC_OPEN_CIRCUIT_V;
138 
139  actuator_controls_s ctrl;
140  orb_copy(ORB_ID(actuator_controls_0), _actuator_ctrl_0_sub, &ctrl);
141  vehicle_control_mode_s vcontrol_mode;
142  orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode);
143 
144  _battery.updateBatteryStatus(t, data.battery_voltage_V, data.battery_current_A,
145  connected, true, 1,
146  ctrl.control[actuator_controls_s::INDEX_THROTTLE],
147  vcontrol_mode.flag_armed, &_battery_status);
148 
149  int instance;
151 
152  return 0;
153 }
154 
155 
157 {
158 
160 
161 int start();
162 int stop();
163 int info();
164 void usage();
165 
166 int start()
167 {
168  PX4_WARN("starting LTC2946");
169  g_dev = new DfLtc2946Wrapper();
170 
171  if (g_dev == nullptr) {
172  PX4_ERR("failed instantiating DfLtc2946Wrapper object");
173  return -1;
174 
175  } else {
176  PX4_INFO("started LTC2946");
177  }
178 
179  int ret = g_dev->start();
180 
181  if (ret != 0) {
182  PX4_ERR("DfLtc2946Wrapper start failed");
183  return ret;
184  }
185 
186  return 0;
187 }
188 
189 int stop()
190 {
191  if (g_dev == nullptr) {
192  PX4_ERR("driver not running");
193  return 1;
194  }
195 
196  int ret = g_dev->stop();
197 
198  if (ret != 0) {
199  PX4_ERR("driver could not be stopped");
200  return ret;
201  }
202 
203  delete g_dev;
204  g_dev = nullptr;
205  return 0;
206 }
207 
208 /**
209  * Print a little info about the driver.
210  */
211 int
213 {
214  if (g_dev == nullptr) {
215  PX4_ERR("driver not running");
216  return 1;
217  }
218 
219  PX4_DEBUG("state @ %p", g_dev);
220 
221  return 0;
222 }
223 
224 void
226 {
227  PX4_WARN("Usage: df_ltc2946_wrapper 'start', 'info', 'stop'");
228 }
229 
230 } // namespace df_ltc2946_wrapper
231 
232 
233 int
234 df_ltc2946_wrapper_main(int argc, char *argv[])
235 {
236  int ret = 0;
237 
238  if (argc <= 1) {
240  return 1;
241  }
242 
243  const char *verb = argv[1];
244 
245 
246  if (!strcmp(verb, "start")) {
248  }
249 
250  else if (!strcmp(verb, "stop")) {
251  ret = df_ltc2946_wrapper::stop();
252  }
253 
254  else if (!strcmp(verb, "info")) {
255  ret = df_ltc2946_wrapper::info();
256  }
257 
258  else {
260  return 1;
261  }
262 
263  return ret;
264 }
orb_advert_t _battery_pub
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
Definition: uORB.cpp:90
API for the uORB lightweight object broker.
void usage(const char *reason)
Print the correct usage.
Definition: Commander.cpp:4238
void reset(battery_status_s *battery_status)
Reset all battery stats and report invalid/nothing.
Definition: battery.cpp:55
Definition: I2C.hpp:51
static void stop()
Definition: dataman.cpp:1491
LidarLite * instance
Definition: ll40ls.cpp:65
High-resolution timer with callouts and timekeeping.
Library calls for battery functionality.
int orb_subscribe(const struct orb_metadata *meta)
Definition: uORB.cpp:75
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
void init()
Activates/configures the hardware registers.
uint8_t * data
Definition: dataman.cpp:149
int orb_unsubscribe(int handle)
Definition: uORB.cpp:85
Simple error/warning functions, heavily inspired by the BSD functions of the same names...
void usage()
Prints info about the driver argument usage.
int stop()
Stop the driver.
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
static int start()
Definition: dataman.cpp:1452
battery_status_s _battery_status
void updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a, bool connected, bool selected_source, int priority, float throttle_normalized, bool armed, battery_status_s *status)
Update current battery status message.
Definition: battery.cpp:68
int _publish(const struct ltc2946_sensor_data &data)
__EXPORT int df_ltc2946_wrapper_main(int argc, char *argv[])
int start()
Attempt to start driver on all available I2C busses.
DfLtc2946Wrapper * g_dev
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
static int orb_publish_auto(const struct orb_metadata *meta, orb_advert_t *handle, const void *data, int *instance, int priority)
Advertise as the publisher of a topic.
Definition: uORB.h:177
int info()
Print a little info about the driver.