PX4 Firmware
PX4 Autopilot Software http://px4.io
dshot.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 <float.h>
35 #include <math.h>
36 
37 #include <board_config.h>
38 #include <drivers/device/device.h>
39 #include <drivers/drv_hrt.h>
41 #include <drivers/drv_mixer.h>
42 #include <drivers/drv_pwm_output.h>
43 #include <lib/cdev/CDev.hpp>
44 #include <lib/mathlib/mathlib.h>
46 #include <lib/parameters/param.h>
47 #include <lib/perf/perf_counter.h>
48 #include <px4_arch/dshot.h>
49 #include <px4_platform_common/atomic.h>
50 #include <px4_platform_common/px4_config.h>
51 #include <px4_platform_common/getopt.h>
52 #include <px4_platform_common/log.h>
53 #include <px4_platform_common/module.h>
54 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
55 #include <uORB/Publication.hpp>
57 #include <uORB/Subscription.hpp>
64 #include <uORB/topics/esc_status.h>
65 
66 #include "telemetry.h"
67 
68 
69 using namespace time_literals;
70 
71 /** Mode given via CLI */
72 enum PortMode {
89 };
90 
91 #if !defined(BOARD_HAS_PWM)
92 # error "board_config.h needs to define BOARD_HAS_PWM"
93 #endif
94 
95 
96 class DShotOutput : public cdev::CDev, public ModuleBase<DShotOutput>, public OutputModuleInterface
97 {
98 public:
99  enum Mode {
117  };
118  DShotOutput();
119  virtual ~DShotOutput();
120 
121  /** @see ModuleBase */
122  static int task_spawn(int argc, char *argv[]);
123 
124  /** @see ModuleBase */
125  static int custom_command(int argc, char *argv[]);
126 
127  /** @see ModuleBase */
128  static int print_usage(const char *reason = nullptr);
129 
130  void Run() override;
131 
132  /** @see ModuleBase::print_status() */
133  int print_status() override;
134 
135  /** change the mode of the running module */
136  static int module_new_mode(PortMode new_mode);
137 
138  virtual int ioctl(file *filp, int cmd, unsigned long arg);
139 
140  virtual int init();
141 
142  int set_mode(Mode mode);
143  Mode get_mode() { return _mode; }
144 
145  static void capture_trampoline(void *context, uint32_t chan_index,
146  hrt_abstime edge_time, uint32_t edge_state,
147  uint32_t overflow);
148 
149  bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
150  unsigned num_outputs, unsigned num_control_groups_updated) override;
151 
152  void mixerChanged() override;
153 
154  /**
155  * Send a dshot command to one or all motors
156  * This is expected to be called from another thread.
157  * @param num_repetitions number of times to repeat, set at least to 1
158  * @param motor_index index or -1 for all
159  * @return 0 on success, <0 error otherwise
160  */
161  int sendCommandThreadSafe(dshot_command_t command, int num_repetitions, int motor_index);
162 
163  void retrieveAndPrintESCInfoThreadSafe(int motor_index);
164 
165  bool telemetryEnabled() const { return _telemetry != nullptr; }
166 
167 private:
168  static constexpr uint16_t DISARMED_VALUE = 0;
169 
170  enum class DShotConfig {
171  Disabled = 0,
172  DShot150 = 150,
173  DShot300 = 300,
174  DShot600 = 600,
175  DShot1200 = 1200,
176  };
177 
178  struct Command {
180  int num_repetitions{0};
181  uint8_t motor_mask{0xff};
182 
183  bool valid() const { return num_repetitions > 0; }
184  void clear() { num_repetitions = 0; }
185  };
186 
187  struct Telemetry {
189  uORB::PublicationData<esc_status_s> esc_status_pub{ORB_ID(esc_status)};
190  int last_motor_index{-1};
191  };
192 
193  void updateTelemetryNumMotors();
194  void initTelemetry(const char *device);
195  void handleNewTelemetryData(int motor_index, const DShotTelemetry::EscData &data);
196 
197  int requestESCInfo();
198 
199  MixingOutput _mixing_output{DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
200 
201  Telemetry *_telemetry{nullptr};
202  static char _telemetry_device[20];
203  static px4::atomic_bool _request_telemetry_init;
204 
205  px4::atomic<DShotTelemetry::OutputBuffer *> _request_esc_info{nullptr};
206  bool _waiting_for_esc_info{false};
207 
208  Mode _mode{MODE_NONE};
209 
211 
213  px4::atomic<Command *> _new_command{nullptr};
214 
215  unsigned _num_outputs{0};
216  int _class_instance{-1};
217 
218  bool _outputs_on{false};
219  uint32_t _output_mask{0};
220  bool _outputs_initialized{false};
221 
223 
224  void capture_callback(uint32_t chan_index,
225  hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow);
226  int pwm_ioctl(file *filp, int cmd, unsigned long arg);
227  void update_dshot_out_state(bool on);
228 
229  void update_params();
230 
231  int capture_ioctl(file *filp, int cmd, unsigned long arg);
232 
233  DShotOutput(const DShotOutput &) = delete;
234  DShotOutput operator=(const DShotOutput &) = delete;
235 
236  DEFINE_PARAMETERS(
237  (ParamInt<px4::params::DSHOT_CONFIG>) _param_dshot_config,
238  (ParamFloat<px4::params::DSHOT_MIN>) _param_dshot_min,
239  (ParamInt<px4::params::MOT_POLE_COUNT>) _param_mot_pole_count
240  )
241 };
242 
244 px4::atomic_bool DShotOutput::_request_telemetry_init{false};
245 
247  CDev("/dev/dshot"),
248  OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default),
249  _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
250 {
251  _mixing_output.setAllDisarmedValues(DISARMED_VALUE);
252  _mixing_output.setAllMinValues(DISARMED_VALUE + 1);
254 
255 }
256 
258 {
259  /* make sure outputs are off */
260  up_dshot_arm(false);
261 
262  /* clean up the alternate device node */
264 
265  perf_free(_cycle_perf);
266  delete _telemetry;
267 }
268 
269 int
271 {
272  /* do regular cdev init */
273  int ret = CDev::init();
274 
275  if (ret != OK) {
276  return ret;
277  }
278 
279  /* try to claim the generic PWM output device node as well - it's OK if we fail at this */
281 
283  /* lets not be too verbose */
284  } else if (_class_instance < 0) {
285  PX4_ERR("FAILED registering class device");
286  }
287 
289 
290  // Getting initial parameter values
291  update_params();
292 
293  ScheduleNow();
294 
295  return 0;
296 }
297 
298 int
300 {
301  unsigned old_mask = _output_mask;
302 
303  /*
304  * Configure for output.
305  *
306  * Note that regardless of the configured mode, the task is always
307  * listening and mixing; the mode just selects which of the channels
308  * are presented on the output pins.
309  */
310  switch (mode) {
311  case MODE_1PWM:
312  /* default output rates */
313  _output_mask = 0x1;
314  _outputs_initialized = false;
315  _num_outputs = 1;
316  break;
317 
318 #if defined(BOARD_HAS_CAPTURE)
319 
320  case MODE_2PWM2CAP: // v1 multi-port with flow control lines as PWM
321  up_input_capture_set(2, Rising, 0, NULL, NULL);
322  up_input_capture_set(3, Rising, 0, NULL, NULL);
323  PX4_DEBUG("MODE_2PWM2CAP");
324 #endif
325 
326  /* FALLTHROUGH */
327 
328  case MODE_2PWM: // v1 multi-port with flow control lines as PWM
329  PX4_DEBUG("MODE_2PWM");
330 
331  /* default output rates */
332  _output_mask = 0x3;
333  _outputs_initialized = false;
334  _num_outputs = 2;
335 
336  break;
337 
338 #if defined(BOARD_HAS_CAPTURE)
339 
340  case MODE_3PWM1CAP: // v1 multi-port with flow control lines as PWM
341  PX4_DEBUG("MODE_3PWM1CAP");
342  up_input_capture_set(3, Rising, 0, NULL, NULL);
343 #endif
344 
345  /* FALLTHROUGH */
346 
347  case MODE_3PWM: // v1 multi-port with flow control lines as PWM
348  PX4_DEBUG("MODE_3PWM");
349 
350  /* default output rates */
351  _output_mask = 0x7;
352  _outputs_initialized = false;
353  _num_outputs = 3;
354 
355  break;
356 
357 #if defined(BOARD_HAS_CAPTURE)
358 
359  case MODE_4PWM1CAP:
360  PX4_DEBUG("MODE_4PWM1CAP");
361  up_input_capture_set(4, Rising, 0, NULL, NULL);
362 #endif
363 
364  /* FALLTHROUGH */
365 
366  case MODE_4PWM: // v1 or v2 multi-port as 4 PWM outs
367  PX4_DEBUG("MODE_4PWM");
368 
369  /* default output rates */
370  _output_mask = 0xf;
371  _outputs_initialized = false;
372  _num_outputs = 4;
373 
374  break;
375 
376 #if defined(BOARD_HAS_CAPTURE)
377 
378  case MODE_4PWM2CAP:
379  PX4_DEBUG("MODE_4PWM2CAP");
380  up_input_capture_set(5, Rising, 0, NULL, NULL);
381 
382  /* default output rates */
383  _output_mask = 0x0f;
384  _outputs_initialized = false;
385  _num_outputs = 4;
386 
387  break;
388 #endif
389 
390 #if defined(BOARD_HAS_CAPTURE)
391 
392  case MODE_5PWM1CAP:
393  PX4_DEBUG("MODE_5PWM1CAP");
394  up_input_capture_set(5, Rising, 0, NULL, NULL);
395 #endif
396 
397  /* FALLTHROUGH */
398 
399  case MODE_5PWM: // v1 or v2 multi-port as 5 PWM outs
400  PX4_DEBUG("MODE_5PWM");
401 
402  /* default output rates */
403  _output_mask = 0x1f;
404  _outputs_initialized = false;
405  _num_outputs = 4;
406 
407  break;
408 
409 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6
410 
411  case MODE_6PWM:
412  PX4_DEBUG("MODE_6PWM");
413 
414  /* default output rates */
415  _output_mask = 0x3f;
416  _outputs_initialized = false;
417  _num_outputs = 6;
418 
419  break;
420 #endif
421 
422 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8
423 
424  case MODE_8PWM: // AeroCore PWMs as 8 PWM outs
425  PX4_DEBUG("MODE_8PWM");
426  /* default output rates */
427  _output_mask = 0xff;
428  _outputs_initialized = false;
429  _num_outputs = 8;
430 
431  break;
432 #endif
433 
434 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 14
435 
436  case MODE_14PWM:
437  PX4_DEBUG("MODE_14PWM");
438  /* default output rates */
439  _output_mask = 0x3fff;
440  _outputs_initialized = false;
441  _num_outputs = 14;
442 
443  break;
444 #endif
445 
446  case MODE_NONE:
447  PX4_DEBUG("MODE_NONE");
448  _output_mask = 0x0;
449  _outputs_initialized = false;
450  _num_outputs = 0;
451 
452  if (old_mask != _output_mask) {
453  /* disable motor outputs */
454  update_dshot_out_state(false);
455  }
456 
457  break;
458 
459  default:
460  return -EINVAL;
461  }
462 
463  _mode = mode;
464  return OK;
465 }
466 
467 int
468 DShotOutput::task_spawn(int argc, char *argv[])
469 {
471 
472  if (instance) {
473  _object.store(instance);
474  _task_id = task_id_is_work_queue;
475 
476  if (instance->init() == PX4_OK) {
477  return PX4_OK;
478  }
479 
480  } else {
481  PX4_ERR("alloc failed");
482  }
483 
484  delete instance;
485  _object.store(nullptr);
486  _task_id = -1;
487 
488  return PX4_ERROR;
489 }
490 
491 void
492 DShotOutput::capture_trampoline(void *context, uint32_t chan_index,
493  hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow)
494 {
495  DShotOutput *dev = static_cast<DShotOutput *>(context);
496  dev->capture_callback(chan_index, edge_time, edge_state, overflow);
497 }
498 
499 void
500 DShotOutput::capture_callback(uint32_t chan_index,
501  hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow)
502 {
503  fprintf(stdout, "DShot: Capture chan:%d time:%lld state:%d overflow:%d\n", chan_index, edge_time, edge_state, overflow);
504 }
505 
506 void
508 {
509  if (on && !_outputs_initialized && _output_mask != 0) {
510  DShotConfig config = (DShotConfig)_param_dshot_config.get();
511  unsigned dshot_frequency;
512 
513  switch (config) {
515  dshot_frequency = DSHOT150;
516  break;
517 
519  dshot_frequency = DSHOT300;
520  break;
521 
523  dshot_frequency = DSHOT1200;
524  break;
525 
527  default:
528  dshot_frequency = DSHOT600;
529  break;
530  }
531 
532  int ret = up_dshot_init(_output_mask, dshot_frequency);
533 
534  if (ret != 0) {
535  PX4_ERR("up_dshot_init failed (%i)", ret);
536  return;
537  }
538 
539  _outputs_initialized = true;
540  }
541 
542  if (_outputs_initialized) {
543  up_dshot_arm(on);
544  _outputs_on = on;
545  }
546 }
547 
549 {
550  if (!_telemetry) {
551  return;
552  }
553 
554  int motor_count = 0;
555 
556  if (_mixing_output.mixers()) {
557  motor_count = _mixing_output.mixers()->get_multirotor_count();
558  }
559 
560  _telemetry->handler.setNumMotors(motor_count);
561 }
562 
563 void DShotOutput::initTelemetry(const char *device)
564 {
565  if (!_telemetry) {
566  _telemetry = new Telemetry{};
567 
568  if (!_telemetry) {
569  PX4_ERR("alloc failed");
570  return;
571  }
572  }
573 
574  int ret = _telemetry->handler.init(device);
575 
576  if (ret != 0) {
577  PX4_ERR("telemetry init failed (%i)", ret);
578  }
579 
581 }
582 
584 {
585  // fill in new motor data
586  esc_status_s &esc_status = _telemetry->esc_status_pub.get();
587 
588  if (motor_index < esc_status_s::CONNECTED_ESC_MAX) {
589  esc_status.esc_online_flags |= 1 << motor_index;
590  esc_status.esc[motor_index].timestamp = data.time;
591  esc_status.esc[motor_index].esc_rpm = ((int)data.erpm * 100) / (_param_mot_pole_count.get() / 2);
592  esc_status.esc[motor_index].esc_voltage = (float)data.voltage * 0.01f;
593  esc_status.esc[motor_index].esc_current = (float)data.current * 0.01f;
594  esc_status.esc[motor_index].esc_temperature = data.temperature;
595  // TODO: accumulate consumption and use for battery estimation
596  }
597 
598  // publish when motor index wraps (which is robust against motor timeouts)
599  if (motor_index <= _telemetry->last_motor_index) {
600  esc_status.timestamp = hrt_absolute_time();
601  esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT;
602  esc_status.esc_count = _telemetry->handler.numMotors();
603  ++esc_status.counter;
604  // FIXME: mark all ESC's as online, otherwise commander complains even for a single dropout
605  esc_status.esc_online_flags = (1 << esc_status.esc_count) - 1;
606 
608 
609  // reset esc data (in case a motor times out, so we won't send stale data)
610  memset(&esc_status.esc, 0, sizeof(_telemetry->esc_status_pub.get().esc));
611  esc_status.esc_online_flags = 0;
612  }
613 
614  _telemetry->last_motor_index = motor_index;
615 }
616 
617 int DShotOutput::sendCommandThreadSafe(dshot_command_t command, int num_repetitions, int motor_index)
618 {
619  Command cmd;
620  cmd.command = command;
621 
622  if (motor_index == -1) {
623  cmd.motor_mask = 0xff;
624 
625  } else {
626  cmd.motor_mask = 1 << _mixing_output.reorderedMotorIndex(motor_index);
627  }
628 
629  cmd.num_repetitions = num_repetitions;
630  _new_command.store(&cmd);
631 
632  // wait until main thread processed it
633  while (_new_command.load()) {
634  px4_usleep(1000);
635  }
636 
637  return 0;
638 }
639 
641 {
642  if (_request_esc_info.load() != nullptr) {
643  // already in progress (not expected to ever happen)
644  return;
645  }
646 
647  DShotTelemetry::OutputBuffer output_buffer;
648  output_buffer.motor_index = motor_index;
649  // start the request
650  _request_esc_info.store(&output_buffer);
651 
652  // wait until processed
653  int max_time = 1000;
654 
655  while (_request_esc_info.load() != nullptr && max_time-- > 0) {
656  px4_usleep(1000);
657  }
658 
659  _request_esc_info.store(nullptr); // just in case we time out...
660 
661  if (output_buffer.buf_pos == 0) {
662  PX4_ERR("No data received. If telemetry is setup correctly, try again");
663  return;
664  }
665 
667 }
668 
670 {
672  _waiting_for_esc_info = true;
673  int motor_index = _mixing_output.reorderedMotorIndex(_request_esc_info.load()->motor_index);
674  _current_command.motor_mask = 1 << motor_index;
675  _current_command.num_repetitions = 1;
676  _current_command.command = DShot_cmd_esc_info;
677  PX4_DEBUG("Requesting ESC info for motor %i", motor_index);
678  return motor_index;
679 }
680 
682 {
684 }
685 
686 bool DShotOutput::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
687  unsigned num_outputs, unsigned num_control_groups_updated)
688 {
689  if (!_outputs_on) {
690  return false;
691  }
692 
693  int requested_telemetry_index = -1;
694 
695  if (_telemetry) {
696  // check for an ESC info request. We only process it when we're not expecting other telemetry data
697  if (_request_esc_info.load() != nullptr && !_waiting_for_esc_info && stop_motors
698  && !_telemetry->handler.expectingData() && !_current_command.valid()) {
699  requested_telemetry_index = requestESCInfo();
700 
701  } else {
703  }
704  }
705 
706  if (stop_motors) {
707 
708  // when motors are stopped we check if we have other commands to send
709  for (int i = 0; i < (int)num_outputs; i++) {
710  if (_current_command.valid() && (_current_command.motor_mask & (1 << i))) {
711  // for some reason we need to always request telemetry when sending a command
712  up_dshot_motor_command(i, _current_command.command, true);
713 
714  } else {
715  up_dshot_motor_command(i, DShot_cmd_motor_stop, i == requested_telemetry_index);
716  }
717  }
718 
719  if (_current_command.valid()) {
720  --_current_command.num_repetitions;
721  }
722 
723  } else {
724  for (int i = 0; i < (int)num_outputs; i++) {
725  if (outputs[i] == DISARMED_VALUE) {
726  up_dshot_motor_command(i, DShot_cmd_motor_stop, i == requested_telemetry_index);
727 
728  } else {
729  up_dshot_motor_data_set(i, math::min(outputs[i], (uint16_t)DSHOT_MAX_THROTTLE), i == requested_telemetry_index);
730  }
731  }
732 
733  // clear commands when motors are running
734  _current_command.clear();
735  }
736 
737  if (stop_motors || num_control_groups_updated > 0) {
739  }
740 
741  return true;
742 }
743 
744 void
746 {
747  if (should_exit()) {
748  ScheduleClear();
750 
751  exit_and_cleanup();
752  return;
753  }
754 
755  perf_begin(_cycle_perf);
756 
758 
759  /* update output status if armed or if mixer is loaded */
760  bool outputs_on = _mixing_output.armed().armed || _mixing_output.mixers();
761 
762  if (_outputs_on != outputs_on) {
763  update_dshot_out_state(outputs_on);
764  }
765 
766  if (_telemetry) {
767  int telem_update = _telemetry->handler.update();
768 
769  // Are we waiting for ESC info?
770  if (_waiting_for_esc_info) {
771  if (telem_update != -1) {
772  _request_esc_info.store(nullptr);
773  _waiting_for_esc_info = false;
774  }
775 
776  } else if (telem_update >= 0) {
778  }
779  }
780 
781  if (_param_sub.updated()) {
782  update_params();
783  }
784 
785  // telemetry device update request?
786  if (_request_telemetry_init.load()) {
787  initTelemetry(_telemetry_device);
788  _request_telemetry_init.store(false);
789  }
790 
791  // new command?
792  if (!_current_command.valid()) {
793  Command *new_command = _new_command.load();
794 
795  if (new_command) {
796  _current_command = *new_command;
797  _new_command.store(nullptr);
798  }
799  }
800 
801  // check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread)
803 
804  perf_end(_cycle_perf);
805 }
806 
808 {
809  parameter_update_s pupdate;
810  _param_sub.update(&pupdate);
811 
812  updateParams();
813 
814  // we use a minimum value of 1, since 0 is for disarmed
815  _mixing_output.setAllMinValues(math::constrain((int)(_param_dshot_min.get() * (float)DSHOT_MAX_THROTTLE),
816  DISARMED_VALUE + 1, DSHOT_MAX_THROTTLE));
817 }
818 
819 
820 int
821 DShotOutput::ioctl(file *filp, int cmd, unsigned long arg)
822 {
823  int ret;
824 
825  /* try it as a Capture ioctl next */
826  ret = capture_ioctl(filp, cmd, arg);
827 
828  if (ret != -ENOTTY) {
829  return ret;
830  }
831 
832  /* if we are in valid PWM mode, try it as a PWM ioctl as well */
833  switch (_mode) {
834  case MODE_1PWM:
835  case MODE_2PWM:
836  case MODE_3PWM:
837  case MODE_4PWM:
838  case MODE_5PWM:
839  case MODE_2PWM2CAP:
840  case MODE_3PWM1CAP:
841  case MODE_4PWM1CAP:
842  case MODE_4PWM2CAP:
843  case MODE_5PWM1CAP:
844 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6
845  case MODE_6PWM:
846 #endif
847 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8
848  case MODE_8PWM:
849 #endif
850 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 14
851  case MODE_14PWM:
852 #endif
853  ret = pwm_ioctl(filp, cmd, arg);
854  break;
855 
856  default:
857  PX4_DEBUG("not in a PWM mode");
858  break;
859  }
860 
861  /* if nobody wants it, let CDev have it */
862  if (ret == -ENOTTY) {
863  ret = CDev::ioctl(filp, cmd, arg);
864  }
865 
866  return ret;
867 }
868 
869 int
870 DShotOutput::pwm_ioctl(file *filp, int cmd, unsigned long arg)
871 {
872  int ret = OK;
873 
874  PX4_DEBUG("dshot ioctl cmd: %d, arg: %ld", cmd, arg);
875 
876  lock();
877 
878  switch (cmd) {
879  case PWM_SERVO_GET_COUNT:
880  switch (_mode) {
881 
882 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 14
883 
884  case MODE_14PWM:
885  *(unsigned *)arg = 14;
886  break;
887 #endif
888 
889 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8
890 
891  case MODE_8PWM:
892  *(unsigned *)arg = 8;
893  break;
894 #endif
895 
896 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6
897 
898  case MODE_6PWM:
899  *(unsigned *)arg = 6;
900  break;
901 #endif
902 
903  case MODE_5PWM:
904  case MODE_5PWM1CAP:
905  *(unsigned *)arg = 5;
906  break;
907 
908  case MODE_4PWM:
909  case MODE_4PWM1CAP:
910  case MODE_4PWM2CAP:
911  *(unsigned *)arg = 4;
912  break;
913 
914  case MODE_3PWM:
915  case MODE_3PWM1CAP:
916  *(unsigned *)arg = 3;
917  break;
918 
919  case MODE_2PWM:
920  case MODE_2PWM2CAP:
921  *(unsigned *)arg = 2;
922  break;
923 
924  case MODE_1PWM:
925  *(unsigned *)arg = 1;
926  break;
927 
928  default:
929  ret = -EINVAL;
930  break;
931  }
932 
933  break;
934 
935  case PWM_SERVO_SET_COUNT: {
936  /* change the number of outputs that are enabled for
937  * PWM. This is used to change the split between GPIO
938  * and PWM under control of the flight config
939  * parameters.
940  */
941  switch (arg) {
942  case 0:
944  break;
945 
946  case 1:
948  break;
949 
950  case 2:
952  break;
953 
954  case 3:
956  break;
957 
958  case 4:
960  break;
961 
962  case 5:
964  break;
965 
966 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >=6
967 
968  case 6:
970  break;
971 #endif
972 
973 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >=8
974 
975  case 8:
977  break;
978 #endif
979 
980  default:
981  ret = -EINVAL;
982  break;
983  }
984 
985  break;
986  }
987 
988  case PWM_SERVO_SET_MODE: {
989  switch (arg) {
990  case PWM_SERVO_MODE_NONE:
991  ret = set_mode(MODE_NONE);
992  break;
993 
994  case PWM_SERVO_MODE_1PWM:
995  ret = set_mode(MODE_1PWM);
996  break;
997 
998  case PWM_SERVO_MODE_2PWM:
999  ret = set_mode(MODE_2PWM);
1000  break;
1001 
1003  ret = set_mode(MODE_2PWM2CAP);
1004  break;
1005 
1006  case PWM_SERVO_MODE_3PWM:
1007  ret = set_mode(MODE_3PWM);
1008  break;
1009 
1011  ret = set_mode(MODE_3PWM1CAP);
1012  break;
1013 
1014  case PWM_SERVO_MODE_4PWM:
1015  ret = set_mode(MODE_4PWM);
1016  break;
1017 
1019  ret = set_mode(MODE_4PWM1CAP);
1020  break;
1021 
1023  ret = set_mode(MODE_4PWM2CAP);
1024  break;
1025 
1026  case PWM_SERVO_MODE_5PWM:
1027  ret = set_mode(MODE_5PWM);
1028  break;
1029 
1031  ret = set_mode(MODE_5PWM1CAP);
1032  break;
1033 
1034  case PWM_SERVO_MODE_6PWM:
1035  ret = set_mode(MODE_6PWM);
1036  break;
1037 
1038  case PWM_SERVO_MODE_8PWM:
1039  ret = set_mode(MODE_8PWM);
1040  break;
1041 
1042  case PWM_SERVO_MODE_4CAP:
1043  ret = set_mode(MODE_4CAP);
1044  break;
1045 
1046  case PWM_SERVO_MODE_5CAP:
1047  ret = set_mode(MODE_5CAP);
1048  break;
1049 
1050  case PWM_SERVO_MODE_6CAP:
1051  ret = set_mode(MODE_6CAP);
1052  break;
1053 
1054  default:
1055  ret = -EINVAL;
1056  }
1057 
1058  break;
1059  }
1060 
1061  case MIXERIOCRESET:
1063 
1064  break;
1065 
1066  case MIXERIOCLOADBUF: {
1067  const char *buf = (const char *)arg;
1068  unsigned buflen = strlen(buf);
1069  ret = _mixing_output.loadMixerThreadSafe(buf, buflen);
1070 
1071  break;
1072  }
1073 
1074  default:
1075  ret = -ENOTTY;
1076  break;
1077  }
1078 
1079  unlock();
1080 
1081  return ret;
1082 }
1083 
1084 int
1085 DShotOutput::capture_ioctl(struct file *filp, int cmd, unsigned long arg)
1086 {
1087  int ret = -EINVAL;
1088 
1089 #if defined(BOARD_HAS_CAPTURE)
1090 
1091  lock();
1092 
1093  input_capture_config_t *pconfig = 0;
1094 
1096 
1097  if (_mode == MODE_3PWM1CAP || _mode == MODE_2PWM2CAP ||
1099  _mode == MODE_4PWM2CAP) {
1100 
1101  pconfig = (input_capture_config_t *)arg;
1102  }
1103 
1104  switch (cmd) {
1105 
1106  case INPUT_CAP_SET:
1107  if (pconfig) {
1108  ret = up_input_capture_set(pconfig->channel, pconfig->edge, pconfig->filter,
1109  pconfig->callback, pconfig->context);
1110  }
1111 
1112  break;
1113 
1115  if (pconfig) {
1116  ret = up_input_capture_set_callback(pconfig->channel, pconfig->callback, pconfig->context);
1117  }
1118 
1119  break;
1120 
1122  if (pconfig) {
1123  ret = up_input_capture_get_callback(pconfig->channel, &pconfig->callback, &pconfig->context);
1124  }
1125 
1126  break;
1127 
1128  case INPUT_CAP_GET_STATS:
1129  if (arg) {
1130  ret = up_input_capture_get_stats(stats->chan_in_edges_out, stats, false);
1131  }
1132 
1133  break;
1134 
1136  if (arg) {
1137  ret = up_input_capture_get_stats(stats->chan_in_edges_out, stats, true);
1138  }
1139 
1140  break;
1141 
1142  case INPUT_CAP_SET_EDGE:
1143  if (pconfig) {
1144  ret = up_input_capture_set_trigger(pconfig->channel, pconfig->edge);
1145  }
1146 
1147  break;
1148 
1149  case INPUT_CAP_GET_EDGE:
1150  if (pconfig) {
1151  ret = up_input_capture_get_trigger(pconfig->channel, &pconfig->edge);
1152  }
1153 
1154  break;
1155 
1156  case INPUT_CAP_SET_FILTER:
1157  if (pconfig) {
1158  ret = up_input_capture_set_filter(pconfig->channel, pconfig->filter);
1159  }
1160 
1161  break;
1162 
1163  case INPUT_CAP_GET_FILTER:
1164  if (pconfig) {
1165  ret = up_input_capture_get_filter(pconfig->channel, &pconfig->filter);
1166  }
1167 
1168  break;
1169 
1170  case INPUT_CAP_GET_COUNT:
1171  ret = OK;
1172 
1173  switch (_mode) {
1174  case MODE_5PWM1CAP:
1175  case MODE_4PWM1CAP:
1176  case MODE_3PWM1CAP:
1177  *(unsigned *)arg = 1;
1178  break;
1179 
1180  case MODE_2PWM2CAP:
1181  case MODE_4PWM2CAP:
1182  *(unsigned *)arg = 2;
1183  break;
1184 
1185  default:
1186  ret = -EINVAL;
1187  break;
1188  }
1189 
1190  break;
1191 
1192  case INPUT_CAP_SET_COUNT:
1193  ret = OK;
1194 
1195  switch (_mode) {
1196  case MODE_3PWM1CAP:
1198  break;
1199 
1200  case MODE_2PWM2CAP:
1202  break;
1203 
1204  case MODE_4PWM1CAP:
1206  break;
1207 
1208  case MODE_4PWM2CAP:
1210  break;
1211 
1212  case MODE_5PWM1CAP:
1214  break;
1215 
1216  default:
1217  ret = -EINVAL;
1218  break;
1219  }
1220 
1221  break;
1222 
1223  default:
1224  ret = -ENOTTY;
1225  break;
1226  }
1227 
1228  unlock();
1229 
1230 #else
1231  ret = -ENOTTY;
1232 #endif
1233  return ret;
1234 }
1235 
1236 int
1238 {
1239  if (!is_running()) {
1240  return -1;
1241  }
1242 
1244 
1245  mode = DShotOutput::MODE_NONE;
1246 
1247  switch (new_mode) {
1248  case PORT_FULL_GPIO:
1249  case PORT_MODE_UNSET:
1250  break;
1251 
1252  case PORT_FULL_PWM:
1253 
1254 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 4
1255  /* select 4-pin PWM mode */
1256  mode = DShotOutput::MODE_4PWM;
1257 #endif
1258 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 6
1259  mode = DShotOutput::MODE_6PWM;
1260 #endif
1261 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 8
1262  mode = DShotOutput::MODE_8PWM;
1263 #endif
1264 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 14
1265  mode = DShotOutput::MODE_14PWM;
1266 #endif
1267  break;
1268 
1269  case PORT_PWM1:
1270  /* select 2-pin PWM mode */
1271  mode = DShotOutput::MODE_1PWM;
1272  break;
1273 
1274 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8
1275 
1276  case PORT_PWM8:
1277  /* select 8-pin PWM mode */
1278  mode = DShotOutput::MODE_8PWM;
1279  break;
1280 #endif
1281 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6
1282 
1283  case PORT_PWM6:
1284  /* select 6-pin PWM mode */
1285  mode = DShotOutput::MODE_6PWM;
1286  break;
1287 
1288  case PORT_PWM5:
1289  /* select 5-pin PWM mode */
1290  mode = DShotOutput::MODE_5PWM;
1291  break;
1292 
1293 
1294 # if defined(BOARD_HAS_CAPTURE)
1295 
1296  case PORT_PWM5CAP1:
1297  /* select 5-pin PWM mode 1 capture */
1299  break;
1300 
1301 # endif
1302 
1303  case PORT_PWM4:
1304  /* select 4-pin PWM mode */
1305  mode = DShotOutput::MODE_4PWM;
1306  break;
1307 
1308 
1309 # if defined(BOARD_HAS_CAPTURE)
1310 
1311  case PORT_PWM4CAP1:
1312  /* select 4-pin PWM mode 1 capture */
1314  break;
1315 
1316  case PORT_PWM4CAP2:
1317  /* select 4-pin PWM mode 2 capture */
1319  break;
1320 
1321 # endif
1322 
1323  case PORT_PWM3:
1324  /* select 3-pin PWM mode */
1325  mode = DShotOutput::MODE_3PWM;
1326  break;
1327 
1328 # if defined(BOARD_HAS_CAPTURE)
1329 
1330  case PORT_PWM3CAP1:
1331  /* select 3-pin PWM mode 1 capture */
1333  break;
1334 # endif
1335 
1336  case PORT_PWM2:
1337  /* select 2-pin PWM mode */
1338  mode = DShotOutput::MODE_2PWM;
1339  break;
1340 
1341 # if defined(BOARD_HAS_CAPTURE)
1342 
1343  case PORT_PWM2CAP2:
1344  /* select 2-pin PWM mode 2 capture */
1346  break;
1347 
1348 # endif
1349 #endif
1350 
1351  default:
1352  return -1;
1353  }
1354 
1355  DShotOutput *object = get_instance();
1356 
1357  if (mode != object->get_mode()) {
1358  /* (re)set the output mode */
1359  object->set_mode(mode);
1360  }
1361 
1362  return OK;
1363 }
1364 
1365 int DShotOutput::custom_command(int argc, char *argv[])
1366 {
1367  PortMode new_mode = PORT_MODE_UNSET;
1368  const char *verb = argv[0];
1369 
1370  if (!strcmp(verb, "telemetry")) {
1371  if (argc > 1) {
1372  // telemetry can be requested before the module is started
1373  strncpy(_telemetry_device, argv[1], sizeof(_telemetry_device) - 1);
1374  _telemetry_device[sizeof(_telemetry_device) - 1] = '\0';
1375  _request_telemetry_init.store(true);
1376  }
1377 
1378  return 0;
1379  }
1380 
1381  int motor_index = -1; // select motor index, default: -1=all
1382  int myoptind = 1;
1383  int ch;
1384  const char *myoptarg = nullptr;
1385 
1386  while ((ch = px4_getopt(argc, argv, "m:", &myoptind, &myoptarg)) != EOF) {
1387  switch (ch) {
1388  case 'm':
1389  motor_index = strtol(myoptarg, nullptr, 10) - 1;
1390  break;
1391 
1392  default:
1393  return print_usage("unrecognized flag");
1394  }
1395  }
1396 
1397  struct Command {
1398  const char *name;
1400  int num_repetitions;
1401  };
1402 
1403  constexpr Command commands[] = {
1404  {"reverse", DShot_cmd_spin_direction_reversed, 10},
1405  {"normal", DShot_cmd_spin_direction_normal, 10},
1406  {"save", DShot_cmd_save_settings, 10},
1407  {"3d_on", DShot_cmd_3d_mode_on, 10},
1408  {"3d_off", DShot_cmd_3d_mode_off, 10},
1409  {"beep1", DShot_cmd_beacon1, 1},
1410  {"beep2", DShot_cmd_beacon2, 1},
1411  {"beep3", DShot_cmd_beacon3, 1},
1412  {"beep4", DShot_cmd_beacon4, 1},
1413  {"beep5", DShot_cmd_beacon5, 1},
1414  };
1415 
1416  for (unsigned i = 0; i < sizeof(commands) / sizeof(commands[0]); ++i) {
1417  if (!strcmp(verb, commands[i].name)) {
1418  if (!is_running()) {
1419  PX4_ERR("module not running");
1420  return -1;
1421  }
1422 
1423  return get_instance()->sendCommandThreadSafe(commands[i].command, commands[i].num_repetitions, motor_index);
1424  }
1425  }
1426 
1427  if (!strcmp(verb, "esc_info")) {
1428  if (!is_running()) {
1429  PX4_ERR("module not running");
1430  return -1;
1431  }
1432 
1433  if (motor_index == -1) {
1434  PX4_ERR("No motor index specified");
1435  return -1;
1436  }
1437 
1438  if (!get_instance()->telemetryEnabled()) {
1439  PX4_ERR("Telemetry is not enabled, but required to get ESC info");
1440  return -1;
1441  }
1442 
1443  get_instance()->retrieveAndPrintESCInfoThreadSafe(motor_index);
1444  return 0;
1445  }
1446 
1447 
1448  if (!is_running()) {
1449  int ret = DShotOutput::task_spawn(argc, argv);
1450 
1451  if (ret) {
1452  return ret;
1453  }
1454  }
1455 
1456  /*
1457  * Mode switches.
1458  */
1459  if (!strcmp(verb, "mode_gpio")) {
1460  new_mode = PORT_FULL_GPIO;
1461 
1462  } else if (!strcmp(verb, "mode_pwm")) {
1463  new_mode = PORT_FULL_PWM;
1464 
1465  // mode: defines which outputs to drive (others may be used by other tasks such as camera capture)
1466 #if defined(BOARD_HAS_PWM)
1467 
1468  } else if (!strcmp(verb, "mode_pwm1")) {
1469  new_mode = PORT_PWM1;
1470 #endif
1471 
1472 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6
1473 
1474  } else if (!strcmp(verb, "mode_pwm6")) {
1475  new_mode = PORT_PWM6;
1476 
1477  } else if (!strcmp(verb, "mode_pwm5")) {
1478  new_mode = PORT_PWM5;
1479 
1480 # if defined(BOARD_HAS_CAPTURE)
1481 
1482  } else if (!strcmp(verb, "mode_pwm5cap1")) {
1483  new_mode = PORT_PWM5CAP1;
1484 # endif
1485 
1486  } else if (!strcmp(verb, "mode_pwm4")) {
1487  new_mode = PORT_PWM4;
1488 
1489 # if defined(BOARD_HAS_CAPTURE)
1490 
1491  } else if (!strcmp(verb, "mode_pwm4cap1")) {
1492  new_mode = PORT_PWM4CAP1;
1493 
1494  } else if (!strcmp(verb, "mode_pwm4cap2")) {
1495  new_mode = PORT_PWM4CAP2;
1496 # endif
1497 
1498  } else if (!strcmp(verb, "mode_pwm3")) {
1499  new_mode = PORT_PWM3;
1500 
1501 # if defined(BOARD_HAS_CAPTURE)
1502 
1503  } else if (!strcmp(verb, "mode_pwm3cap1")) {
1504  new_mode = PORT_PWM3CAP1;
1505 # endif
1506 
1507  } else if (!strcmp(verb, "mode_pwm2")) {
1508  new_mode = PORT_PWM2;
1509 
1510 # if defined(BOARD_HAS_CAPTURE)
1511 
1512  } else if (!strcmp(verb, "mode_pwm2cap2")) {
1513  new_mode = PORT_PWM2CAP2;
1514 # endif
1515 #endif
1516 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8
1517 
1518  } else if (!strcmp(verb, "mode_pwm8")) {
1519  new_mode = PORT_PWM8;
1520 #endif
1521  }
1522 
1523  /* was a new mode set? */
1524  if (new_mode != PORT_MODE_UNSET) {
1525 
1526  /* switch modes */
1527  return DShotOutput::module_new_mode(new_mode);
1528  }
1529 
1530  return print_usage("unknown command");
1531 }
1532 
1534 {
1535  const char *mode_str = nullptr;
1536 
1537  switch (_mode) {
1538 
1539  case MODE_NONE: mode_str = "no outputs"; break;
1540 
1541  case MODE_1PWM: mode_str = "outputs1"; break;
1542 
1543  case MODE_2PWM: mode_str = "outputs2"; break;
1544 
1545  case MODE_2PWM2CAP: mode_str = "outputs2cap2"; break;
1546 
1547  case MODE_3PWM: mode_str = "outputs3"; break;
1548 
1549  case MODE_3PWM1CAP: mode_str = "outputs3cap1"; break;
1550 
1551  case MODE_4PWM: mode_str = "outputs4"; break;
1552 
1553  case MODE_4PWM1CAP: mode_str = "outputs4cap1"; break;
1554 
1555  case MODE_4PWM2CAP: mode_str = "outputs4cap2"; break;
1556 
1557  case MODE_5PWM: mode_str = "outputs5"; break;
1558 
1559  case MODE_5PWM1CAP: mode_str = "outputs5cap1"; break;
1560 
1561  case MODE_6PWM: mode_str = "outputs6"; break;
1562 
1563  case MODE_8PWM: mode_str = "outputs8"; break;
1564 
1565  case MODE_4CAP: mode_str = "cap4"; break;
1566 
1567  case MODE_5CAP: mode_str = "cap5"; break;
1568 
1569  case MODE_6CAP: mode_str = "cap6"; break;
1570 
1571  default:
1572  break;
1573  }
1574 
1575  if (mode_str) {
1576  PX4_INFO("Mode: %s", mode_str);
1577  }
1578 
1579  PX4_INFO("Outputs initialized: %s", _outputs_initialized ? "yes" : "no");
1580  PX4_INFO("Outputs on: %s", _outputs_on ? "yes" : "no");
1581  perf_print_counter(_cycle_perf);
1583 
1584  if (_telemetry) {
1585  PX4_INFO("telemetry on: %s", _telemetry_device);
1587  }
1588 
1589  return 0;
1590 }
1591 
1592 int DShotOutput::print_usage(const char *reason)
1593 {
1594  if (reason) {
1595  PX4_WARN("%s\n", reason);
1596  }
1597 
1598  PRINT_MODULE_DESCRIPTION(
1599  R"DESCR_STR(
1600 ### Description
1601 This is the DShot output driver. It is similar to the fmu driver, and can be used as drop-in replacement
1602 to use DShot as ESC communication protocol instead of PWM.
1603 
1604 It supports:
1605 - DShot150, DShot300, DShot600, DShot1200
1606 - telemetry via separate UART and publishing as esc_status message
1607 - sending DShot commands via CLI
1608 
1609 ### Examples
1610 Permanently reverse motor 1:
1611 $ dshot reverse -m 1
1612 $ dshot save -m 1
1613 After saving, the reversed direction will be regarded as the normal one. So to reverse again repeat the same commands.
1614 )DESCR_STR");
1615 
1616  PRINT_MODULE_USAGE_NAME("dshot", "driver");
1617  PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the task (without any mode set, use any of the mode_* cmds)");
1618 
1619  PRINT_MODULE_USAGE_PARAM_COMMENT("All of the mode_* commands will start the module if not running already");
1620 
1621  PRINT_MODULE_USAGE_COMMAND("mode_gpio");
1622  PRINT_MODULE_USAGE_COMMAND_DESCR("mode_pwm", "Select all available pins as PWM");
1623 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8
1624  PRINT_MODULE_USAGE_COMMAND("mode_pwm8");
1625 #endif
1626 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6
1627  PRINT_MODULE_USAGE_COMMAND("mode_pwm6");
1628  PRINT_MODULE_USAGE_COMMAND("mode_pwm5");
1629  PRINT_MODULE_USAGE_COMMAND("mode_pwm5cap1");
1630  PRINT_MODULE_USAGE_COMMAND("mode_pwm4");
1631  PRINT_MODULE_USAGE_COMMAND("mode_pwm4cap1");
1632  PRINT_MODULE_USAGE_COMMAND("mode_pwm4cap2");
1633  PRINT_MODULE_USAGE_COMMAND("mode_pwm3");
1634  PRINT_MODULE_USAGE_COMMAND("mode_pwm3cap1");
1635  PRINT_MODULE_USAGE_COMMAND("mode_pwm2");
1636  PRINT_MODULE_USAGE_COMMAND("mode_pwm2cap2");
1637 #endif
1638 #if defined(BOARD_HAS_PWM)
1639  PRINT_MODULE_USAGE_COMMAND("mode_pwm1");
1640 #endif
1641 
1642  PRINT_MODULE_USAGE_COMMAND_DESCR("telemetry", "Enable Telemetry on a UART");
1643  PRINT_MODULE_USAGE_ARG("<device>", "UART device", false);
1644 
1645  // DShot commands
1646  PRINT_MODULE_USAGE_COMMAND_DESCR("reverse", "Reverse motor direction");
1647  PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true);
1648  PRINT_MODULE_USAGE_COMMAND_DESCR("normal", "Normal motor direction");
1649  PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true);
1650  PRINT_MODULE_USAGE_COMMAND_DESCR("save", "Save current settings");
1651  PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true);
1652  PRINT_MODULE_USAGE_COMMAND_DESCR("3d_on", "Enable 3D mode");
1653  PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true);
1654  PRINT_MODULE_USAGE_COMMAND_DESCR("3d_off", "Disable 3D mode");
1655  PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true);
1656  PRINT_MODULE_USAGE_COMMAND_DESCR("beep1", "Send Beep pattern 1");
1657  PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true);
1658  PRINT_MODULE_USAGE_COMMAND_DESCR("beep2", "Send Beep pattern 2");
1659  PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true);
1660  PRINT_MODULE_USAGE_COMMAND_DESCR("beep3", "Send Beep pattern 3");
1661  PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true);
1662  PRINT_MODULE_USAGE_COMMAND_DESCR("beep4", "Send Beep pattern 4");
1663  PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true);
1664  PRINT_MODULE_USAGE_COMMAND_DESCR("beep5", "Send Beep pattern 5");
1665  PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true);
1666 
1667  PRINT_MODULE_USAGE_COMMAND_DESCR("esc_info", "Request ESC information");
1668  PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based)", false);
1669 
1670  PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
1671 
1672  return 0;
1673 }
1674 
1675 extern "C" __EXPORT int dshot_main(int argc, char *argv[])
1676 {
1677  return DShotOutput::main(argc, argv);
1678 }
__EXPORT void up_dshot_trigger(void)
Trigger dshot data transfer.
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
Definition: Limits.hpp:66
__EXPORT int up_input_capture_set_callback(unsigned channel, capture_callback_t callback, void *context)
#define PWM_SERVO_MODE_2PWM2CAP
struct esc_report_s esc[8]
Definition: esc_status.h:67
perf_counter_t _cycle_perf
Definition: dshot.cpp:222
#define MIXERIOCLOADBUF
Add mixer(s) from the buffer in (const char *)arg.
Definition: drv_mixer.h:79
px4::atomic< DShotTelemetry::OutputBuffer * > _request_esc_info
Definition: dshot.cpp:205
bool telemetryEnabled() const
Definition: dshot.cpp:165
int numMotors() const
Definition: telemetry.h:68
int16_t current
[0.01A]
Definition: telemetry.h:45
const actuator_armed_s & armed() const
#define DSHOT1200
Dshot PWM frequency.
uint8_t esc_online_flags
Definition: esc_status.h:65
MixingOutput _mixing_output
Definition: dshot.cpp:199
void printStatus() const
static px4::atomic_bool _request_telemetry_init
Definition: dshot.cpp:203
void initTelemetry(const char *device)
Definition: dshot.cpp:563
#define PWM_SERVO_MODE_2PWM
measure the time elapsed performing an event
Definition: perf_counter.h:56
static int print_usage(const char *reason=nullptr)
Definition: dshot.cpp:1592
#define PWM_SERVO_MODE_4PWM
int16_t erpm
[100ERPM]
Definition: telemetry.h:47
#define PWM_SERVO_GET_COUNT
get the number of servos in *(unsigned *)arg
unsigned get_multirotor_count()
Definition: MixerGroup.cpp:139
#define INPUT_CAP_GET_STATS
Get channel stats - arg is pointer to input_capture_config with channel set.
virtual int ioctl(file *filp, int cmd, unsigned long arg)
Definition: dshot.cpp:821
int _class_instance
Definition: dshot.cpp:216
#define PWM_SERVO_MODE_8PWM
#define PWM_SERVO_MODE_5PWM1CAP
uORB::PublicationData< esc_status_s > esc_status_pub
Definition: dshot.cpp:189
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
int main(int argc, char **argv)
Definition: main.cpp:3
input_capture_edge edge
#define INPUT_CAP_GET_EDGE
Get Edge for a channel arg is pointer to input_capture_config with channel set.
void lock()
Take the driver lock.
Definition: CDev.hpp:264
int set_mode(Mode mode)
Definition: dshot.cpp:299
int pwm_ioctl(file *filp, int cmd, unsigned long arg)
Definition: dshot.cpp:870
uint8_t esc_connectiontype
Definition: esc_status.h:64
static int task_spawn(int argc, char *argv[])
Definition: dshot.cpp:468
Definition: I2C.hpp:51
float esc_current
Definition: esc_report.h:57
int getRequestMotorIndex()
Get the motor index for which telemetry should be requested.
Definition: telemetry.cpp:231
static Mode _mode
Definition: motor_ramp.cpp:81
void setNumMotors(int num_motors)
Definition: telemetry.h:67
void retrieveAndPrintESCInfoThreadSafe(int motor_index)
Definition: dshot.cpp:640
__EXPORT int up_input_capture_set_trigger(unsigned channel, input_capture_edge edge)
uint64_t timestamp
Definition: esc_status.h:61
void print_status()
Definition: Commander.cpp:517
#define PWM_SERVO_MODE_NONE
set auxillary output mode.
virtual int init()
Definition: dshot.cpp:270
bool update()
Call this regularly from Run().
void mixerChanged() override
called whenever the mixer gets updated/reset
Definition: dshot.cpp:681
int sendCommandThreadSafe(dshot_command_t command, int num_repetitions, int motor_index)
Send a dshot command to one or all motors This is expected to be called from another thread...
Definition: dshot.cpp:617
static void print_usage()
int32_t esc_rpm
Definition: esc_report.h:55
bool _outputs_on
Definition: dshot.cpp:218
int8_t temperature
[deg C]
Definition: telemetry.h:43
int capture_ioctl(file *filp, int cmd, unsigned long arg)
Definition: dshot.cpp:1085
static bool is_running()
Definition: dataman.cpp:415
#define PWM_SERVO_MODE_4PWM1CAP
LidarLite * instance
Definition: ll40ls.cpp:65
dshot_command_t command
Definition: dshot.cpp:179
Namespace encapsulating all device framework classes, functions and data.
Definition: CDev.cpp:47
bool updateSubscriptions(bool allow_wq_switch)
Check for subscription updates (e.g.
uint32_t _output_mask
Definition: dshot.cpp:219
Command _current_command
Definition: dshot.cpp:212
#define DSHOT_MAX_THROTTLE
#define INPUT_CAP_SET_CALLBACK
Set the call back on a capture channel - arg is pointer to input_capture_config with channel call bac...
High-resolution timer with callouts and timekeeping.
void printStatus() const
Definition: telemetry.cpp:194
px4::atomic< Command * > _new_command
Definition: dshot.cpp:213
Global flash based parameter store.
void update_params()
Definition: dshot.cpp:807
static int custom_command(int argc, char *argv[])
Definition: dshot.cpp:1365
Mixer ioctl interfaces.
__EXPORT int up_input_capture_get_trigger(unsigned channel, input_capture_edge *edge)
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
#define PWM_SERVO_MODE_6PWM
#define PWM_SERVO_MODE_3PWM
__EXPORT int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq)
Intialise the Dshot outputs using the specified configuration.
#define INPUT_CAP_SET
Set Enable a channel arg is pointer to input_capture_config with all parameters set.
__EXPORT int up_input_capture_get_callback(unsigned channel, capture_callback_t *callback, void **context)
__EXPORT int up_dshot_arm(bool armed)
Arm or disarm dshot outputs (This will enable/disable complete timer for safety purpose.).
Abstract class for any character device.
Definition: CDev.hpp:58
#define INPUT_CAP_GET_FILTER
Set Filter input filter channel arg is pointer to input_capture_config with channel set...
Drive scheduling based on subscribed actuator controls topics (via uORB callbacks) ...
Header common to all counters.
#define DSHOT150
DShotTelemetry handler
Definition: dshot.cpp:188
uint64_t timestamp
Definition: esc_report.h:53
void perf_free(perf_counter_t handle)
Free a counter.
void init()
Activates/configures the hardware registers.
uORB::Subscription _param_sub
Definition: dshot.cpp:210
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
uint8_t esc_temperature
Definition: esc_report.h:58
#define PWM_SERVO_MODE_5PWM
#define perf_alloc(a, b)
Definition: px4io.h:59
int update()
Read telemetry from the UART (non-blocking) and handle timeouts.
Definition: telemetry.cpp:90
Mode get_mode()
Definition: dshot.cpp:143
#define INPUT_CAP_GET_CLR_STATS
Get channel stats - arg is pointer to input_capture_config with channel set.
uint8_t * data
Definition: dataman.cpp:149
#define PWM_SERVO_MODE_5CAP
DShotOutput()
Definition: dshot.cpp:246
__EXPORT int up_input_capture_get_filter(unsigned channel, capture_filter_t *filter)
float esc_voltage
Definition: esc_report.h:56
const EscData & latestESCData() const
Definition: telemetry.h:94
dshot_command_t
#define PWM_SERVO_MODE_4CAP
Telemetry * _telemetry
Definition: dshot.cpp:201
void resetMixerThreadSafe()
Reset (unload) the complete mixer, called from another thread.
#define INPUT_CAP_SET_EDGE
Set Edge a channel arg is pointer to input_capture_config with channel and edge set.
uint8_t motor_mask
Definition: dshot.cpp:181
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override
Callback to update the (physical) actuator outputs in the driver.
Definition: dshot.cpp:686
virtual ~DShotOutput()
Definition: dshot.cpp:257
static void capture_trampoline(void *context, uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow)
Definition: dshot.cpp:492
PortMode
Mode given via CLI.
Definition: dshot.cpp:72
Mode _mode
Definition: dshot.cpp:208
void perf_end(perf_counter_t handle)
End a performance event.
#define PWM_SERVO_MODE_6CAP
bool expectingData() const
Check whether we are currently expecting to read new data from an ESC.
Definition: telemetry.h:99
int reorderedMotorIndex(int index) const
Get the motor index that maps from PX4 convention to the configured one.
__EXPORT int up_input_capture_set(unsigned channel, input_capture_edge edge, capture_filter_t filter, capture_callback_t callback, void *context)
bool updated()
Check if there is a new update.
Base class for an output module.
#define INPUT_CAP_SET_COUNT
Set the number of capture in (unsigned)arg - allows change of split between servos and capture...
bool _waiting_for_esc_info
Definition: dshot.cpp:206
uint16_t counter
Definition: esc_status.h:62
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
int loadMixerThreadSafe(const char *buf, unsigned len)
Load (append) a new mixer from a buffer, called from another thread.
int requestESCInfo()
Definition: dshot.cpp:669
__EXPORT void up_dshot_motor_command(unsigned channel, uint16_t command, bool telemetry)
Send DShot command to a channel (motor).
void handleNewTelemetryData(int motor_index, const DShotTelemetry::EscData &data)
Definition: dshot.cpp:583
#define PWM_SERVO_MODE_4PWM2CAP
constexpr _Tp min(_Tp a, _Tp b)
Definition: Limits.hpp:54
#define DSHOT300
#define INPUT_CAP_GET_COUNT
Get the number of capture in *(unsigned *)arg.
#define MIXERIOCRESET
reset (clear) the mixer configuration
Definition: drv_mixer.h:71
static void update_params(ParameterHandles &param_handles, Parameters &params, bool &got_changes)
Definition: vmount.cpp:525
uint8_t esc_count
Definition: esc_status.h:63
__EXPORT int dshot_main(int argc, char *argv[])
Definition: dshot.cpp:1675
void setAllMaxValues(uint16_t value)
#define DSHOT600
const char * name
Definition: tests_main.c:58
int16_t voltage
[0.01V]
Definition: telemetry.h:44
input capture values for a channel
#define PWM_SERVO_SET_MODE
static char _telemetry_device[20]
Definition: dshot.cpp:202
int redirectOutput(OutputBuffer &buffer)
Redirect everything that is read into a different buffer.
Definition: telemetry.cpp:76
#define PWM_OUTPUT_BASE_DEVICE_PATH
Path for the default PWM output device.
__EXPORT void up_dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry)
Set the current dshot throttle value for a channel (motor).
#define INPUT_CAP_SET_FILTER
Set Filter input filter channel arg is pointer to input_capture_config with channel and filter set...
#define INPUT_CAP_GET_CALLBACK
Get the call back on a capture channel - arg is pointer to input_capture_config with channel set...
struct @83::@85::@87 file
capture_callback_t callback
__EXPORT int up_input_capture_get_stats(unsigned channel, input_capture_stats_t *stats, bool clear)
__EXPORT int up_input_capture_set_filter(unsigned channel, capture_filter_t filter)
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
void setAllDisarmedValues(uint16_t value)
static int module_new_mode(PortMode new_mode)
change the mode of the running module
Definition: dshot.cpp:1237
void setDriverInstance(uint8_t instance)
Definition: bst.cpp:62
void updateTelemetryNumMotors()
Definition: dshot.cpp:548
int init(const char *uart_device)
Definition: telemetry.cpp:52
bool valid() const
Definition: dshot.cpp:183
void Run() override
Definition: dshot.cpp:745
static void decodeAndPrintEscInfoPacket(const OutputBuffer &buffer)
Definition: telemetry.cpp:242
#define OK
Definition: uavcan_main.cpp:71
bool update(void *dst)
Update the struct.
MixerGroup * mixers() const
void unregister()
unregister uORB subscription callbacks
#define PWM_SERVO_MODE_1PWM
bool _outputs_initialized
Definition: dshot.cpp:220
int print_status() override
Definition: dshot.cpp:1533
void unlock()
Release the driver lock.
Definition: CDev.hpp:269
mode
Definition: vtol_type.h:76
#define PWM_SERVO_MODE_3PWM1CAP
void perf_begin(perf_counter_t handle)
Begin a performance event.
This handles the mixing, arming/disarming and all subscriptions required for that.
#define PWM_SERVO_SET_COUNT
set the number of servos in (unsigned)arg - allows change of split between servos and GPIO ...
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
void update_dshot_out_state(bool on)
Definition: dshot.cpp:507
Performance measuring tools.
void setAllMinValues(uint16_t value)
unsigned _num_outputs
Definition: dshot.cpp:215
void capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow)
Definition: dshot.cpp:500