PX4 Firmware
PX4 Autopilot Software http://px4.io
snapdragon_pwm_out.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2015-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 #include <stdint.h>
35 
36 #include <px4_platform_common/tasks.h>
37 #include <px4_platform_common/getopt.h>
38 #include <px4_platform_common/posix.h>
39 #include <errno.h>
40 #include <math.h> // NAN
41 #include <string.h>
42 
43 #include <uORB/Subscription.hpp>
48 
49 #include <drivers/drv_hrt.h>
50 #include <drivers/drv_mixer.h>
52 #include <lib/mixer/MixerGroup.hpp>
53 #include <lib/mixer/mixer_load.h>
54 #include <lib/parameters/param.h>
55 #include <lib/perf/perf_counter.h>
57 
58 #include <dev_fs_lib_pwm.h>
59 
60 /*
61  * PWM Driver for snapdragon.
62  * @author: Dennis Mannhart <dennis.mannhart@gmail.com>
63  */
64 namespace snapdragon_pwm
65 {
66 
67 static px4_task_t _task_handle = -1;
68 volatile bool _task_should_exit = false;
69 static bool _is_running = false;
70 static const int NUM_PWM = 4;
71 static char _device[] = "/dev/pwm-1";
72 
73 // snapdragon pins 27,28,29,30 configured for pwm (port J13)
74 static const int PIN_GPIO = 27;
75 static struct ::dspal_pwm_ioctl_signal_definition _signal_definition;
76 static struct ::dspal_pwm _pwm_gpio[NUM_PWM];
77 static struct ::dspal_pwm_ioctl_update_buffer *_update_buffer;
78 static struct ::dspal_pwm *_pwm;
79 
80 // TODO: check frequency required
81 static const int FREQUENCY_PWM_HZ = 400;
82 
83 /* TODO: copied from pwm_out_rc_in: check how that works
84  * filename: /dev/fs is mapped to /usr/share/data/adsp */
85 static char _mixer_filename[32] = "/dev/fs/quad_x.main.mix";
86 
87 // subscriptions
88 int _controls_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
90 int _fd = -1;
91 
92 // publications
94 
95 // topic structures
96 actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
97 orb_id_t _controls_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
100 
101 // polling
102 uint8_t _poll_fds_num = 0;
103 px4_pollfd_struct_t _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
104 
105 // control group
106 uint32_t _groups_required = 0;
107 
108 // limit for pwm
110 
111 // esc parameters
113 int32_t _pwm_min;
114 int32_t _pwm_max;
115 
117 
119 
120 /*
121  * forward declaration
122  */
123 
124 static void usage();
125 
126 static void start();
127 
128 static void stop();
129 
130 static int pwm_initialize(const char *device);
131 
132 static void pwm_deinitialize();
133 
134 static void send_outputs_pwm(const uint16_t *pwm);
135 
136 static void task_main_trampoline(int argc, char *argv[]);
137 
138 static void subscribe();
139 
140 static void task_main(int argc, char *argv[]);
141 
142 static void update_params(Mixer::Airmode &airmode);
143 
144 int initialize_mixer(const char *mixer_filename);
145 
146 int mixer_control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input);
147 
148 
149 
150 /*
151  * functions
152  */
153 
154 int mixer_control_callback(uintptr_t handle,
155  uint8_t control_group,
156  uint8_t control_index,
157  float &input)
158 {
159  const actuator_controls_s *controls = (actuator_controls_s *)handle;
160 
161  input = controls[control_group].control[control_index];
162 
163  return 0;
164 }
165 
167 {
168  // multicopter air-mode
169  param_t param_handle = param_find("MC_AIRMODE");
170 
171  if (param_handle != PARAM_INVALID) {
172  param_get(param_handle, &airmode);
173  }
174 }
175 
176 int initialize_mixer(const char *mixer_filename)
177 {
178  char buf[2048];
179  size_t buflen = sizeof(buf);
180  PX4_INFO("Trying to initialize mixer from config file %s", mixer_filename);
181  int fd_load = ::open(mixer_filename, O_RDONLY);
182 
183  int nRead = ::read(fd_load, buf, buflen);
184  close(fd_load);
185 
186  if (nRead > 0) {
187  _mixer = MultirotorMixer::from_text(mixer_control_callback, (uintptr_t)&_controls, buf, buflen);
188 
189  if (_mixer != nullptr) {
190  PX4_INFO("Successfully initialized mixer from config file");
191  return 0;
192 
193  } else {
194  PX4_ERR("Unable to parse from mixer config file");
195  return -1;
196  }
197 
198  } else {
199  PX4_WARN("Unable to read from mixer config file");
200  return -2;
201  }
202 }
203 
204 void subscribe()
205 {
206  memset(_controls, 0, sizeof(_controls));
207  memset(_poll_fds, 0, sizeof(_poll_fds));
208 
209  /*
210  * ORB topic names to msg actuator_controls
211  */
212  _controls_topics[0] = ORB_ID(actuator_controls_0);
213  _controls_topics[1] = ORB_ID(actuator_controls_1);
214  _controls_topics[2] = ORB_ID(actuator_controls_2);
215  _controls_topics[3] = ORB_ID(actuator_controls_3);
216 
217 
218  /*
219  * subscibe to orb topic
220  */
221  for (uint8_t i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
222 
223  if (_groups_required & (1 << i)) {
224 
225  PX4_DEBUG("subscribe to actuator_controls_%d", i);
226 
227  _controls_subs[i] = orb_subscribe(_controls_topics[i]);
228 
229  } else {
230 
231  _controls_subs[i] = -1;
232 
233  }
234 
235  if (_controls_subs[i] >= 0) {
236 
237  _poll_fds[_poll_fds_num].fd = _controls_subs[i];
238  _poll_fds[_poll_fds_num].events = POLLIN;
239  _poll_fds_num++;
240 
241  }
242 
243  }
244 
245  _armed_sub = orb_subscribe(ORB_ID(actuator_armed));
246 
247 }
248 
249 
250 
251 int pwm_initialize(const char *device)
252 {
253  /*
254  * open PWM device
255  */
256  _fd = open(_device, 0);
257 
258  if (_fd < 0) {
259  PX4_ERR("failed to open PWM device!");
260  return -1;
261  }
262 
263  /*
264  * configure PWM
265  */
266  for (int i = 0; i < NUM_PWM; i++) {
267  _pwm_gpio[i].gpio_id = PIN_GPIO + i;
268  }
269 
270  /*
271  * description of signal
272  */
273  _signal_definition.num_gpios = NUM_PWM;
274  _signal_definition.period_in_usecs = 1000000 / FREQUENCY_PWM_HZ;
275  _signal_definition.pwm_signal = _pwm_gpio;
276 
277 
278  /*
279  * send signal definition to DSP
280  */
281  if (::ioctl(_fd, PWM_IOCTL_SIGNAL_DEFINITION, &_signal_definition) != 0) {
282  PX4_ERR("failed to send signal to DSP");
283  return -1;
284  }
285 
286  /*
287  * retrieve shared buffer which will be used to update desired pulse width
288  */
289  if (::ioctl(_fd, PWM_IOCTL_GET_UPDATE_BUFFER, &_update_buffer) != 0) {
290  PX4_ERR("failed to receive update buffer ");
291  return -1;
292  }
293 
294  _pwm = _update_buffer->pwm_signal;
295 
296 
297  return 0;
298 }
299 
301 {
302  delete _mixer;
303  close(_fd);
304 
305 }
306 
307 void send_outputs_pwm(const uint16_t *pwm)
308 {
309  /*
310  * send pwm in us: TODO: check if it is in us
311  */
312  for (unsigned i = 0; i < NUM_PWM; ++i) {
313  _pwm[i].pulse_width_in_usecs = pwm[i];
314  }
315 }
316 
317 void task_main(int argc, char *argv[])
318 {
319  if (pwm_initialize(_device) < 0) {
320  PX4_ERR("Failed to initialize PWM.");
321  return;
322  }
323 
324  if (initialize_mixer(_mixer_filename) < 0) {
325  PX4_ERR("Mixer initialization failed.");
326  return;
327  }
328 
329  _mixer->groups_required(_groups_required);
330 
331  // subscribe and set up polling
332  subscribe();
333 
335  update_params(airmode);
336  uORB::Subscription parameter_update_sub{ORB_ID(parameter_update)};
337 
338  // Start disarmed
339  _armed.armed = false;
340  _armed.prearmed = false;
341 
342  // set max min pwm
343  output_limit_init(&_pwm_limit);
344 
345  _perf_control_latency = perf_alloc(PC_ELAPSED, "snapdragon_pwm_out control latency");
346 
347  _is_running = true;
348 
349  // Main loop
350  while (!_task_should_exit) {
351 
352  if (_mixer) {
353  _mixer->set_airmode(airmode);
354  }
355 
356  /* wait up to 10ms for data */
357  int pret = px4_poll(_poll_fds, _poll_fds_num, 10);
358 
359  /* Timed out, do a periodic check for _task_should_exit. */
360  bool timeout = false;
361 
362  if (pret == 0) {
363  timeout = true;
364  }
365 
366  /* This is undesirable but not much we can do. */
367  if (pret < 0) {
368  PX4_WARN("poll error %d, %d", pret, errno);
369  /* sleep a bit before next try */
370  usleep(10000);
371  continue;
372  }
373 
374  bool updated;
375  orb_check(_armed_sub, &updated);
376 
377  if (updated) {
378  orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
379  }
380 
381  /* get controls for required topics */
382  unsigned poll_id = 0;
383 
384  for (uint8_t i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
385  if (_controls_subs[i] >= 0) {
386  if (_poll_fds[poll_id].revents & POLLIN) {
387  orb_copy(_controls_topics[i], _controls_subs[i], &_controls[i]);
388  }
389 
390  poll_id++;
391  }
392  }
393 
394  if (_mixer == nullptr) {
395  PX4_ERR("Could not mix output! Exiting...");
396  _task_should_exit = true;
397  continue;
398  }
399 
400  /* do mixing for virtual control group */
401  _outputs.noutputs = _mixer->mix(_outputs.output, _outputs.NUM_ACTUATOR_OUTPUTS);
402 
403  //set max, min and disarmed pwm
404  const uint16_t reverse_mask = 0;
405  uint16_t disarmed_pwm[4];
406  uint16_t min_pwm[4];
407  uint16_t max_pwm[4];
408  uint16_t pwm[4];
409 
410  for (unsigned int i = 0; i < 4; i++) {
411  disarmed_pwm[i] = _pwm_disarmed;
412  min_pwm[i] = _pwm_min;
413  max_pwm[i] = _pwm_max;
414  }
415 
416 
417  // TODO FIXME: pre-armed seems broken -> copied and pasted from pwm_out_rc_in: needs to be tested
418  output_limit_calc(_armed.armed,
419  false/*_armed.prearmed*/, _outputs.noutputs, reverse_mask, disarmed_pwm,
420  min_pwm, max_pwm, _outputs.output, pwm, &_pwm_limit);
421 
422  // send and publish outputs
423  if (_armed.lockdown || _armed.manual_lockdown || timeout) {
424  send_outputs_pwm(disarmed_pwm);
425 
426  } else {
427  send_outputs_pwm(pwm);
428  }
429 
430  _outputs.timestamp = hrt_absolute_time();
431 
432  if (_outputs_pub != nullptr) {
433  orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs);
434 
435  } else {
436  _outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs);
437  }
438 
439  // use first valid timestamp_sample for latency tracking
440  for (int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
441  const bool required = _groups_required & (1 << i);
442  const hrt_abstime &timestamp_sample = _controls[i].timestamp_sample;
443 
444  if (required && (timestamp_sample > 0)) {
445  perf_set_elapsed(_perf_control_latency, _outputs.timestamp - timestamp_sample);
446  break;
447  }
448  }
449 
450  // check for parameter updates
451  if (parameter_update_sub.updated()) {
452  // clear update
453  parameter_update_s pupdate;
454  parameter_update_sub.copy(&pupdate);
455 
456  // update parameters from storage
457  update_params(airmode);
458  }
459  }
460 
462 
463  for (uint8_t i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
464  if (_controls_subs[i] >= 0) {
465  orb_unsubscribe(_controls_subs[i]);
466  }
467  }
468 
469  orb_unsubscribe(_armed_sub);
470 
471  perf_free(_perf_control_latency);
472 
473  _is_running = false;
474 }
475 
476 void task_main_trampoline(int argc, char *argv[])
477 {
478  task_main(argc, argv);
479 }
480 
481 void start()
482 {
483  _task_should_exit = false;
484 
485  /* start the task */
486  _task_handle = px4_task_spawn_cmd("snapdragon_pwm_out_main",
487  SCHED_DEFAULT,
488  SCHED_PRIORITY_MAX,
489  1500,
490  (px4_main_t)&task_main_trampoline,
491  nullptr);
492 
493  if (_task_handle < 0) {
494  PX4_ERR("task start failed");
495  return;
496  }
497 
498 }
499 
500 void stop()
501 {
502  _task_should_exit = true;
503 
504  while (_is_running) {
505  usleep(200000);
506  PX4_INFO(".");
507  }
508 
509  _task_handle = -1;
510 }
511 
512 void usage()
513 {
514 
515  PX4_INFO("usage: snapdragon_pwm_out start [-d pwmdevice] [-m mixerfile]");
516  PX4_INFO(" -d pwmdevice : device for pwm generation");
517  PX4_INFO(" (default /dev/pwm-1)");
518  PX4_INFO(" -m mixerfile : path to mixerfile");
519  PX4_INFO(" (default /dev/fs/quad_x.main.mix");
520  PX4_INFO(" pwm_out stop");
521  PX4_INFO(" pwm_out status");
522 
523 }
524 
525 } // namespace snapdragon_pwm
526 
527 /* driver 'main' command */
528 extern "C" __EXPORT int snapdragon_pwm_out_main(int argc, char *argv[]);
529 
530 int snapdragon_pwm_out_main(int argc, char *argv[])
531 {
532 
533 
534  int ch;
535  int myoptind = 1;
536  const char *myoptarg = nullptr;
537 
538  char *verb = nullptr;
539 
540  if (argc >= 2) {
541  verb = argv[1];
542 
543  } else {
544  return 1;
545  }
546 
547  while ((ch = px4_getopt(argc, argv, "d:m", &myoptind, &myoptarg)) != EOF) {
548  switch (ch) {
549  case 'd':
550  strncpy(snapdragon_pwm::_device, myoptarg, sizeof(snapdragon_pwm::_device));
551  break;
552 
553  case 'm':
555  break;
556  }
557  }
558 
559  // gets the parameters for the esc's pwm
563 
564  /*
565  * Start/load the driver.
566  */
567  if (!strcmp(verb, "start")) {
569  PX4_WARN("pwm_out already running");
570  return 1;
571  }
572 
574  }
575 
576  else if (!strcmp(verb, "stop")) {
578  PX4_WARN("pwm_out is not running");
579  return 1;
580  }
581 
583  }
584 
585  else if (!strcmp(verb, "status")) {
586  PX4_INFO("pwm_out is %s", snapdragon_pwm::_is_running ? "running" : "not running");
587  return 0;
588 
589  } else {
591  return 1;
592  }
593 
594  return 0;
595 }
int initialize_mixer(const char *mixer_filename)
#define PARAM_INVALID
Handle returned when a parameter cannot be found.
Definition: param.h:103
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
Definition: uORB.cpp:90
static const int FREQUENCY_PWM_HZ
void output_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, const uint16_t reverse_mask, const uint16_t *disarmed_output, const uint16_t *min_output, const uint16_t *max_output, const float *output, uint16_t *effective_output, output_limit_t *limit)
measure the time elapsed performing an event
Definition: perf_counter.h:56
MultirotorMixer * _mixer
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
Definition: parameters.cpp:589
static MultirotorMixer * from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
Factory method.
perf_counter_t _perf_control_latency
Definition: I2C.hpp:51
int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
void perf_set_elapsed(perf_counter_t handle, int64_t elapsed)
Register a measurement.
orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data)
Definition: uORB.cpp:43
static char _mixer_filename[32]
Namespace encapsulating all device framework classes, functions and data.
Definition: CDev.cpp:47
High-resolution timer with callouts and timekeeping.
static struct ::dspal_pwm _pwm_gpio[NUM_PWM]
Global flash based parameter store.
void set_airmode(Airmode airmode) override
Set airmode.
Mixer ioctl interfaces.
__EXPORT int snapdragon_pwm_out_main(int argc, char *argv[])
static void read(bootloader_app_shared_t *pshared)
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
static px4_task_t _task_handle
static void task_main_trampoline(int argc, char *argv[])
Header common to all counters.
void perf_free(perf_counter_t handle)
Free a counter.
orb_advert_t _outputs_pub
#define perf_alloc(a, b)
Definition: px4io.h:59
static char _device[]
int orb_unsubscribe(int handle)
Definition: uORB.cpp:85
orb_id_t _controls_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]
static void task_main(int argc, char *argv[])
actuator_armed_s _armed
static void subscribe()
static struct ::dspal_pwm * _pwm
Airmode
Definition: Mixer.hpp:139
px4_pollfd_struct_t _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]
int _controls_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]
__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
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
Definition: uORB.cpp:70
static void pwm_deinitialize()
int mixer_control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input)
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
Definition: parameters.cpp:370
static const int PIN_GPIO
Object metadata.
Definition: uORB.h:50
unsigned mix(float *outputs, unsigned space) override
Perform the mixing function.
volatile bool _task_should_exit
output_limit_t _pwm_limit
int orb_check(int handle, bool *updated)
Definition: uORB.cpp:95
static struct ::dspal_pwm_ioctl_signal_definition _signal_definition
void output_limit_init(output_limit_t *limit)
static struct ::dspal_pwm_ioctl_update_buffer * _update_buffer
void groups_required(uint32_t &groups) override
Analyses the mix configuration and updates a bitmask of groups that are required. ...
Multi-rotor mixer for pre-defined vehicle geometries.
static const int NUM_PWM
actuator_outputs_s _outputs
static void send_outputs_pwm(const uint16_t *pwm)
static bool _is_running
Library for output limiting (PWM for example)
static void update_params(Mixer::Airmode &airmode)
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
uint32_t param_t
Parameter handle.
Definition: param.h:98
actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]
Performance measuring tools.
static int pwm_initialize(const char *device)