PX4 Firmware
PX4 Autopilot Software http://px4.io
fmu.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2012-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 /**
35  * @file fmu.cpp
36  *
37  * Driver/configurator for the PX4 FMU
38  */
39 
40 #include <float.h>
41 #include <math.h>
42 
43 #include <board_config.h>
44 #include <drivers/device/device.h>
45 #include <drivers/device/i2c.h>
46 #include <drivers/drv_hrt.h>
48 #include <drivers/drv_mixer.h>
49 #include <drivers/drv_pwm_output.h>
50 #include <lib/cdev/CDev.hpp>
51 #include <lib/mathlib/mathlib.h>
53 #include <lib/parameters/param.h>
54 #include <lib/perf/perf_counter.h>
55 #include <px4_platform_common/px4_config.h>
56 #include <px4_platform_common/getopt.h>
57 #include <px4_platform_common/log.h>
58 #include <px4_platform_common/module.h>
59 #include <uORB/Publication.hpp>
61 #include <uORB/Subscription.hpp>
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 #define PX4FMU_DEVICE_PATH "/dev/px4fmu"
96 
97 
98 class PX4FMU : public cdev::CDev, public ModuleBase<PX4FMU>, public OutputModuleInterface
99 {
100 public:
101  enum Mode {
119  };
120  PX4FMU();
121  virtual ~PX4FMU();
122 
123  /** @see ModuleBase */
124  static int task_spawn(int argc, char *argv[]);
125 
126  /** @see ModuleBase */
127  static int custom_command(int argc, char *argv[]);
128 
129  /** @see ModuleBase */
130  static int print_usage(const char *reason = nullptr);
131 
132  void Run() override;
133 
134  /** @see ModuleBase::print_status() */
135  int print_status() override;
136 
137  /** change the FMU mode of the running module */
138  static int fmu_new_mode(PortMode new_mode);
139 
140  static int test();
141 
142  static int fake(int argc, char *argv[]);
143 
144  virtual int ioctl(file *filp, int cmd, unsigned long arg);
145 
146  virtual int init();
147 
148  int set_mode(Mode mode);
149  Mode get_mode() { return _mode; }
150 
151  static int set_i2c_bus_clock(unsigned bus, unsigned clock_hz);
152 
153  static void capture_trampoline(void *context, uint32_t chan_index,
154  hrt_abstime edge_time, uint32_t edge_state,
155  uint32_t overflow);
156 
157  void update_pwm_trims();
158 
159  bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
160  unsigned num_outputs, unsigned num_control_groups_updated) override;
161 
162 private:
163  static constexpr int FMU_MAX_ACTUATORS = DIRECT_PWM_OUTPUT_CHANNELS;
164  static_assert(FMU_MAX_ACTUATORS <= MAX_ACTUATORS, "Increase MAX_ACTUATORS if this fails");
165 
166  MixingOutput _mixing_output{FMU_MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, true};
167 
168  Mode _mode{MODE_NONE};
169 
170  unsigned _pwm_default_rate{50};
171  unsigned _pwm_alt_rate{50};
172  uint32_t _pwm_alt_rate_channels{0};
173 
174  unsigned _current_update_rate{0};
175 
176  uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
177 
178 
179  unsigned _num_outputs{0};
180  int _class_instance{-1};
181 
182  bool _pwm_on{false};
183  uint32_t _pwm_mask{0};
184  bool _pwm_initialized{false};
185  bool _test_mode{false};
186 
187  unsigned _num_disarmed_set{0};
188 
190 
191  void capture_callback(uint32_t chan_index,
192  hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow);
193  void update_current_rate();
194  int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate);
195  int pwm_ioctl(file *filp, int cmd, unsigned long arg);
196  void update_pwm_rev_mask();
197  void update_pwm_out_state(bool on);
198 
199  void update_params();
200 
201  static void sensor_reset(int ms);
202  static void peripheral_reset(int ms);
203 
204  int capture_ioctl(file *filp, int cmd, unsigned long arg);
205 
206  PX4FMU(const PX4FMU &) = delete;
207  PX4FMU operator=(const PX4FMU &) = delete;
208 
209 };
210 
212  CDev(PX4FMU_DEVICE_PATH),
213  OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default),
214  _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
215 {
218 
219 }
220 
222 {
223  /* make sure servos are off */
225 
226  /* clean up the alternate device node */
228 
230 }
231 
232 int
234 {
235  /* do regular cdev init */
236  int ret = CDev::init();
237 
238  if (ret != OK) {
239  return ret;
240  }
241 
242  // XXX best would be to register / de-register the device depending on modes
243 
244  /* try to claim the generic PWM output device node as well - it's OK if we fail at this */
246 
248  /* lets not be too verbose */
249  } else if (_class_instance < 0) {
250  PX4_ERR("FAILED registering class device");
251  }
252 
254 
255  /* force a reset of the update rate */
257 
258  // Getting initial parameter values
259  update_params();
260 
261  ScheduleNow();
262 
263  return 0;
264 }
265 
266 int
268 {
269  unsigned old_mask = _pwm_mask;
270 
271  /*
272  * Configure for PWM output.
273  *
274  * Note that regardless of the configured mode, the task is always
275  * listening and mixing; the mode just selects which of the channels
276  * are presented on the output pins.
277  */
278  switch (mode) {
279  case MODE_1PWM:
280  /* default output rates */
281  _pwm_default_rate = 50;
282  _pwm_alt_rate = 50;
284  _pwm_mask = 0x1;
285  _pwm_initialized = false;
286  _num_outputs = 1;
287  break;
288 
289 #if defined(BOARD_HAS_CAPTURE)
290 
291  case MODE_2PWM2CAP: // v1 multi-port with flow control lines as PWM
292  up_input_capture_set(2, Rising, 0, NULL, NULL);
293  up_input_capture_set(3, Rising, 0, NULL, NULL);
294  PX4_DEBUG("MODE_2PWM2CAP");
295 #endif
296 
297  /* FALLTHROUGH */
298 
299  case MODE_2PWM: // v1 multi-port with flow control lines as PWM
300  PX4_DEBUG("MODE_2PWM");
301 
302  /* default output rates */
303  _pwm_default_rate = 50;
304  _pwm_alt_rate = 50;
306  _pwm_mask = 0x3;
307  _pwm_initialized = false;
308  _num_outputs = 2;
309 
310  break;
311 
312 #if defined(BOARD_HAS_CAPTURE)
313 
314  case MODE_3PWM1CAP: // v1 multi-port with flow control lines as PWM
315  PX4_DEBUG("MODE_3PWM1CAP");
316  up_input_capture_set(3, Rising, 0, NULL, NULL);
317 #endif
318 
319  /* FALLTHROUGH */
320 
321  case MODE_3PWM: // v1 multi-port with flow control lines as PWM
322  PX4_DEBUG("MODE_3PWM");
323 
324  /* default output rates */
325  _pwm_default_rate = 50;
326  _pwm_alt_rate = 50;
328  _pwm_mask = 0x7;
329  _pwm_initialized = false;
330  _num_outputs = 3;
331 
332  break;
333 
334 #if defined(BOARD_HAS_CAPTURE)
335 
336  case MODE_4PWM1CAP:
337  PX4_DEBUG("MODE_4PWM1CAP");
338  up_input_capture_set(4, Rising, 0, NULL, NULL);
339 #endif
340 
341  /* FALLTHROUGH */
342 
343  case MODE_4PWM: // v1 or v2 multi-port as 4 PWM outs
344  PX4_DEBUG("MODE_4PWM");
345 
346  /* default output rates */
347  _pwm_default_rate = 50;
348  _pwm_alt_rate = 50;
350  _pwm_mask = 0xf;
351  _pwm_initialized = false;
352  _num_outputs = 4;
353 
354  break;
355 
356 #if defined(BOARD_HAS_CAPTURE)
357 
358  case MODE_4PWM2CAP:
359  PX4_DEBUG("MODE_4PWM2CAP");
360  up_input_capture_set(5, Rising, 0, NULL, NULL);
361 
362  /* default output rates */
363  _pwm_default_rate = 400;
364  _pwm_alt_rate = 50;
366  _pwm_mask = 0x0f;
367  _pwm_initialized = false;
368  _num_outputs = 4;
369 
370  break;
371 #endif
372 
373 #if defined(BOARD_HAS_CAPTURE)
374 
375  case MODE_5PWM1CAP:
376  PX4_DEBUG("MODE_5PWM1CAP");
377  up_input_capture_set(5, Rising, 0, NULL, NULL);
378 #endif
379 
380  /* FALLTHROUGH */
381 
382  case MODE_5PWM: // v1 or v2 multi-port as 5 PWM outs
383  PX4_DEBUG("MODE_5PWM");
384 
385  /* default output rates */
386  _pwm_default_rate = 50;
387  _pwm_alt_rate = 50;
389  _pwm_mask = 0x1f;
390  _pwm_initialized = false;
391  _num_outputs = 4;
392 
393  break;
394 
395 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6
396 
397  case MODE_6PWM:
398  PX4_DEBUG("MODE_6PWM");
399 
400  /* default output rates */
401  _pwm_default_rate = 50;
402  _pwm_alt_rate = 50;
404  _pwm_mask = 0x3f;
405  _pwm_initialized = false;
406  _num_outputs = 6;
407 
408  break;
409 #endif
410 
411 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8
412 
413  case MODE_8PWM: // AeroCore PWMs as 8 PWM outs
414  PX4_DEBUG("MODE_8PWM");
415  /* default output rates */
416  _pwm_default_rate = 50;
417  _pwm_alt_rate = 50;
419  _pwm_mask = 0xff;
420  _pwm_initialized = false;
421  _num_outputs = 8;
422 
423  break;
424 #endif
425 
426 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 14
427 
428  case MODE_14PWM:
429  PX4_DEBUG("MODE_14PWM");
430  /* default output rates */
431  _pwm_default_rate = 50;
432  _pwm_alt_rate = 50;
434  _pwm_mask = 0x3fff;
435  _pwm_initialized = false;
436  _num_outputs = 14;
437 
438  break;
439 #endif
440 
441  case MODE_NONE:
442  PX4_DEBUG("MODE_NONE");
443 
444  _pwm_default_rate = 10; /* artificially reduced output rate */
445  _pwm_alt_rate = 10;
447  _pwm_mask = 0x0;
448  _pwm_initialized = false;
449  _num_outputs = 0;
450 
451  if (old_mask != _pwm_mask) {
452  /* disable servo outputs - no need to set rates */
454  }
455 
456  break;
457 
458  default:
459  return -EINVAL;
460  }
461 
462  _mode = mode;
463  return OK;
464 }
465 
466 /* When set_pwm_rate is called from either of the 2 IOCTLs:
467  *
468  * PWM_SERVO_SET_UPDATE_RATE - Sets the "alternate" channel's rate to the callers's rate specified
469  * and the non "alternate" channels to the _pwm_default_rate.
470  *
471  * rate_map = _pwm_alt_rate_channels
472  * default_rate = _pwm_default_rate
473  * alt_rate = arg of IOCTL (see rates)
474  *
475  * PWM_SERVO_SET_SELECT_UPDATE_RATE - The caller's specified rate map selects the "alternate" channels
476  * to be set to the alt rate. (_pwm_alt_rate)
477  * All other channels are set to the default rate. (_pwm_default_rate)
478  *
479  * rate_map = arg of IOCTL
480  * default_rate = _pwm_default_rate
481  * alt_rate = _pwm_alt_rate
482 
483  * rate_map - A mask of 1's for the channels to be set to the
484  * alternate rate.
485  * N.B. All channels is a given group must be set
486  * to the same rate/mode. (default or alt)
487  * rates:
488  * alt_rate, default_rate For PWM is 25 or 400Hz
489  * For Oneshot there is no rate, 0 is therefore used
490  * to select Oneshot mode
491  */
492 int
493 PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate)
494 {
495  PX4_DEBUG("set_pwm_rate %x %u %u", rate_map, default_rate, alt_rate);
496 
497  for (unsigned pass = 0; pass < 2; pass++) {
498 
499  /* We should note that group is iterated over from 0 to FMU_MAX_ACTUATORS.
500  * This allows for the ideal worlds situation: 1 channel per group
501  * configuration.
502  *
503  * This is typically not what HW supports. A group represents a timer
504  * and channels belongs to a timer.
505  * Therefore all channels in a group are dependent on the timer's
506  * common settings and can not be independent in terms of count frequency
507  * (granularity of pulse width) and rate (period of repetition).
508  *
509  * To say it another way, all channels in a group moust have the same
510  * rate and mode. (See rates above.)
511  */
512 
513  for (unsigned group = 0; group < FMU_MAX_ACTUATORS; group++) {
514 
515  // get the channel mask for this rate group
516  uint32_t mask = up_pwm_servo_get_rate_group(group);
517 
518  if (mask == 0) {
519  continue;
520  }
521 
522  // all channels in the group must be either default or alt-rate
523  uint32_t alt = rate_map & mask;
524 
525  if (pass == 0) {
526  // preflight
527  if ((alt != 0) && (alt != mask)) {
528  PX4_WARN("rate group %u mask %x bad overlap %x", group, mask, alt);
529  // not a legal map, bail
530  return -EINVAL;
531  }
532 
533  } else {
534  // set it - errors here are unexpected
535  if (alt != 0) {
536  if (up_pwm_servo_set_rate_group_update(group, alt_rate) != OK) {
537  PX4_WARN("rate group set alt failed");
538  return -EINVAL;
539  }
540 
541  } else {
542  if (up_pwm_servo_set_rate_group_update(group, default_rate) != OK) {
543  PX4_WARN("rate group set default failed");
544  return -EINVAL;
545  }
546  }
547  }
548  }
549  }
550 
551  _pwm_alt_rate_channels = rate_map;
552  _pwm_default_rate = default_rate;
553  _pwm_alt_rate = alt_rate;
554 
555  _current_update_rate = 0; // force update
556 
557  return OK;
558 }
559 
560 int
561 PX4FMU::set_i2c_bus_clock(unsigned bus, unsigned clock_hz)
562 {
563  return device::I2C::set_bus_clock(bus, clock_hz);
564 }
565 
566 void
568 {
569  /*
570  * Adjust actuator topic update rate to keep up with
571  * the highest servo update rate configured.
572  *
573  * We always mix at max rate; some channels may update slower.
574  */
576 
577  // oneshot
578  if ((_pwm_default_rate == 0) || (_pwm_alt_rate == 0)) {
579  max_rate = 2000;
580  }
581 
582  // max interval 0.5 - 100 ms (10 - 2000Hz)
583  const int update_interval_in_us = math::constrain(1000000 / max_rate, 500, 100000);
584 
585  _current_update_rate = max_rate;
586  _mixing_output.setMaxTopicUpdateRate(update_interval_in_us);
587 }
588 
589 void
591 {
592  uint16_t &reverse_pwm_mask = _mixing_output.reverseOutputMask();
593  reverse_pwm_mask = 0;
594 
595  const char *pname_format;
596 
598  pname_format = "PWM_MAIN_REV%d";
599 
600  } else if (_class_instance == CLASS_DEVICE_SECONDARY) {
601  pname_format = "PWM_AUX_REV%d";
602 
603  } else {
604  PX4_ERR("PWM REV only for MAIN and AUX");
605  return;
606  }
607 
608  for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) {
609  char pname[16];
610 
611  /* fill the channel reverse mask from parameters */
612  sprintf(pname, pname_format, i + 1);
613  param_t param_h = param_find(pname);
614 
615  if (param_h != PARAM_INVALID) {
616  int32_t ival = 0;
617  param_get(param_h, &ival);
618  reverse_pwm_mask |= ((int16_t)(ival != 0)) << i;
619  }
620  }
621 }
622 
623 void
625 {
626  PX4_DEBUG("update_pwm_trims");
627 
628  if (!_mixing_output.mixers()) {
629  return;
630  }
631 
632  int16_t values[FMU_MAX_ACTUATORS] = {};
633 
634  const char *pname_format;
635 
637  pname_format = "PWM_MAIN_TRIM%d";
638 
639  } else if (_class_instance == CLASS_DEVICE_SECONDARY) {
640  pname_format = "PWM_AUX_TRIM%d";
641 
642  } else {
643  PX4_ERR("PWM TRIM only for MAIN and AUX");
644  return;
645  }
646 
647  for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) {
648  char pname[16];
649 
650  /* fill the struct from parameters */
651  sprintf(pname, pname_format, i + 1);
652  param_t param_h = param_find(pname);
653 
654  if (param_h != PARAM_INVALID) {
655  float pval = 0.0f;
656  param_get(param_h, &pval);
657  values[i] = (int16_t)(10000 * pval);
658  PX4_DEBUG("%s: %d", pname, values[i]);
659  }
660  }
661 
662  /* copy the trim values to the mixer offsets */
663  unsigned n_out = _mixing_output.mixers()->set_trims(values, FMU_MAX_ACTUATORS);
664  PX4_DEBUG("set %d trims", n_out);
665 }
666 
667 int
668 PX4FMU::task_spawn(int argc, char *argv[])
669 {
670  PX4FMU *instance = new PX4FMU();
671 
672  if (instance) {
673  _object.store(instance);
674  _task_id = task_id_is_work_queue;
675 
676  if (instance->init() == PX4_OK) {
677  return PX4_OK;
678  }
679 
680  } else {
681  PX4_ERR("alloc failed");
682  }
683 
684  delete instance;
685  _object.store(nullptr);
686  _task_id = -1;
687 
688  return PX4_ERROR;
689 }
690 
691 void
692 PX4FMU::capture_trampoline(void *context, uint32_t chan_index,
693  hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow)
694 {
695  PX4FMU *dev = static_cast<PX4FMU *>(context);
696  dev->capture_callback(chan_index, edge_time, edge_state, overflow);
697 }
698 
699 void
700 PX4FMU::capture_callback(uint32_t chan_index,
701  hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow)
702 {
703  fprintf(stdout, "FMU: Capture chan:%d time:%lld state:%d overflow:%d\n", chan_index, edge_time, edge_state, overflow);
704 }
705 
706 void
708 {
709  if (on && !_pwm_initialized && _pwm_mask != 0) {
712  _pwm_initialized = true;
713  }
714 
715  up_pwm_servo_arm(on);
716 }
717 
718 
719 bool PX4FMU::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
720  unsigned num_outputs, unsigned num_control_groups_updated)
721 {
722  if (_test_mode) {
723  return false;
724  }
725 
726  /* output to the servos */
727  if (_pwm_initialized) {
728  for (size_t i = 0; i < num_outputs; i++) {
729  up_pwm_servo_set(i, outputs[i]);
730  }
731  }
732 
733  /* Trigger all timer's channels in Oneshot mode to fire
734  * the oneshots with updated values.
735  */
736  if (num_control_groups_updated > 0) {
737  up_pwm_update();
738  }
739 
740  return true;
741 }
742 
743 void
745 {
746  if (should_exit()) {
747  ScheduleClear();
749 
750  exit_and_cleanup();
751  return;
752  }
753 
755 
757 
758  /* update PWM status if armed or if disarmed PWM values are set */
760 
761  if (_pwm_on != pwm_on) {
762  _pwm_on = pwm_on;
763  update_pwm_out_state(pwm_on);
764  }
765 
766  // check for parameter updates
768  // clear update
769  parameter_update_s pupdate;
770  _parameter_update_sub.copy(&pupdate);
771 
772  // update parameters from storage
773  update_params();
774  }
775 
776  if (_current_update_rate == 0) {
778  }
779 
780  // check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread)
782 
784 }
785 
787 {
790 
791  updateParams();
792 }
793 
794 int
795 PX4FMU::ioctl(file *filp, int cmd, unsigned long arg)
796 {
797  int ret;
798 
799  /* try it as a Capture ioctl next */
800  ret = capture_ioctl(filp, cmd, arg);
801 
802  if (ret != -ENOTTY) {
803  return ret;
804  }
805 
806  /* if we are in valid PWM mode, try it as a PWM ioctl as well */
807  switch (_mode) {
808  case MODE_1PWM:
809  case MODE_2PWM:
810  case MODE_3PWM:
811  case MODE_4PWM:
812  case MODE_5PWM:
813  case MODE_2PWM2CAP:
814  case MODE_3PWM1CAP:
815  case MODE_4PWM1CAP:
816  case MODE_4PWM2CAP:
817  case MODE_5PWM1CAP:
818 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6
819  case MODE_6PWM:
820 #endif
821 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8
822  case MODE_8PWM:
823 #endif
824 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 14
825  case MODE_14PWM:
826 #endif
827  ret = pwm_ioctl(filp, cmd, arg);
828  break;
829 
830  default:
831  PX4_DEBUG("not in a PWM mode");
832  break;
833  }
834 
835  /* if nobody wants it, let CDev have it */
836  if (ret == -ENOTTY) {
837  ret = CDev::ioctl(filp, cmd, arg);
838  }
839 
840  return ret;
841 }
842 
843 int
844 PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
845 {
846  int ret = OK;
847 
848  PX4_DEBUG("fmu ioctl cmd: %d, arg: %ld", cmd, arg);
849 
850  lock();
851 
852  switch (cmd) {
853  case PWM_SERVO_ARM:
854  update_pwm_out_state(true);
855  break;
856 
861  break;
862 
863  case PWM_SERVO_DISARM:
864 
865  /* Ignore disarm if disarmed PWM is set already. */
866  if (_num_disarmed_set == 0) {
867  update_pwm_out_state(false);
868  }
869 
870  break;
871 
873  *(uint32_t *)arg = _pwm_default_rate;
874  break;
875 
878  break;
879 
881  *(uint32_t *)arg = _pwm_alt_rate;
882  break;
883 
886  break;
887 
889  *(uint32_t *)arg = _pwm_alt_rate_channels;
890  break;
891 
893  struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
894 
895  /* discard if too many values are sent */
896  if (pwm->channel_count > FMU_MAX_ACTUATORS) {
897  ret = -EINVAL;
898  break;
899  }
900 
901  for (unsigned i = 0; i < pwm->channel_count; i++) {
902  if (pwm->values[i] == 0) {
903  /* ignore 0 */
904  } else if (pwm->values[i] > PWM_HIGHEST_MAX) {
906 
907  }
908 
909 #if PWM_LOWEST_MIN > 0
910 
911  else if (pwm->values[i] < PWM_LOWEST_MIN) {
913 
914  }
915 
916 #endif
917 
918  else {
919  _mixing_output.failsafeValue(i) = pwm->values[i];
920  }
921  }
922 
923  break;
924  }
925 
927  struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
928 
929  for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) {
930  pwm->values[i] = _mixing_output.failsafeValue(i);
931  }
932 
934  break;
935  }
936 
938  struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
939 
940  /* discard if too many values are sent */
941  if (pwm->channel_count > FMU_MAX_ACTUATORS) {
942  ret = -EINVAL;
943  break;
944  }
945 
946  for (unsigned i = 0; i < pwm->channel_count; i++) {
947  if (pwm->values[i] == 0) {
948  /* ignore 0 */
949  } else if (pwm->values[i] > PWM_HIGHEST_MAX) {
951  }
952 
953 #if PWM_LOWEST_MIN > 0
954 
955  else if (pwm->values[i] < PWM_LOWEST_MIN) {
957  }
958 
959 #endif
960 
961  else {
962  _mixing_output.disarmedValue(i) = pwm->values[i];
963  }
964  }
965 
966  /*
967  * update the counter
968  * this is needed to decide if disarmed PWM output should be turned on or not
969  */
970  _num_disarmed_set = 0;
971 
972  for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) {
973  if (_mixing_output.disarmedValue(i) > 0) {
975  }
976  }
977 
978  break;
979  }
980 
982  struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
983 
984  for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) {
985  pwm->values[i] = _mixing_output.disarmedValue(i);
986  }
987 
989  break;
990  }
991 
992  case PWM_SERVO_SET_MIN_PWM: {
993  struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
994 
995  /* discard if too many values are sent */
996  if (pwm->channel_count > FMU_MAX_ACTUATORS) {
997  ret = -EINVAL;
998  break;
999  }
1000 
1001  for (unsigned i = 0; i < pwm->channel_count; i++) {
1002  if (pwm->values[i] == 0) {
1003  /* ignore 0 */
1004  } else if (pwm->values[i] > PWM_HIGHEST_MIN) {
1006 
1007  }
1008 
1009 #if PWM_LOWEST_MIN > 0
1010 
1011  else if (pwm->values[i] < PWM_LOWEST_MIN) {
1013  }
1014 
1015 #endif
1016 
1017  else {
1018  _mixing_output.minValue(i) = pwm->values[i];
1019  }
1020  }
1021 
1022  break;
1023  }
1024 
1025  case PWM_SERVO_GET_MIN_PWM: {
1026  struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
1027 
1028  for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) {
1029  pwm->values[i] = _mixing_output.minValue(i);
1030  }
1031 
1033  arg = (unsigned long)&pwm;
1034  break;
1035  }
1036 
1037  case PWM_SERVO_SET_MAX_PWM: {
1038  struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
1039 
1040  /* discard if too many values are sent */
1041  if (pwm->channel_count > FMU_MAX_ACTUATORS) {
1042  ret = -EINVAL;
1043  break;
1044  }
1045 
1046  for (unsigned i = 0; i < pwm->channel_count; i++) {
1047  if (pwm->values[i] == 0) {
1048  /* ignore 0 */
1049  } else if (pwm->values[i] < PWM_LOWEST_MAX) {
1051 
1052  } else if (pwm->values[i] > PWM_HIGHEST_MAX) {
1054 
1055  } else {
1056  _mixing_output.maxValue(i) = pwm->values[i];
1057  }
1058  }
1059 
1060  break;
1061  }
1062 
1063  case PWM_SERVO_GET_MAX_PWM: {
1064  struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
1065 
1066  for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) {
1067  pwm->values[i] = _mixing_output.maxValue(i);
1068  }
1069 
1071  arg = (unsigned long)&pwm;
1072  break;
1073  }
1074 
1075  case PWM_SERVO_SET_TRIM_PWM: {
1076  struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
1077 
1078  /* discard if too many values are sent */
1079  if (pwm->channel_count > FMU_MAX_ACTUATORS) {
1080  PX4_DEBUG("error: too many trim values: %d", pwm->channel_count);
1081  ret = -EINVAL;
1082  break;
1083  }
1084 
1085  if (_mixing_output.mixers() == nullptr) {
1086  PX4_ERR("error: no mixer loaded");
1087  ret = -EIO;
1088  break;
1089  }
1090 
1091  /* copy the trim values to the mixer offsets */
1092  _mixing_output.mixers()->set_trims((int16_t *)pwm->values, pwm->channel_count);
1093  PX4_DEBUG("set_trims: %d, %d, %d, %d", pwm->values[0], pwm->values[1], pwm->values[2], pwm->values[3]);
1094 
1095  break;
1096  }
1097 
1098  case PWM_SERVO_GET_TRIM_PWM: {
1099  struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
1100 
1101  if (_mixing_output.mixers() == nullptr) {
1102  memset(pwm, 0, sizeof(pwm_output_values));
1103  PX4_WARN("warning: trim values not valid - no mixer loaded");
1104 
1105  } else {
1106 
1107  pwm->channel_count = _mixing_output.mixers()->get_trims((int16_t *)pwm->values);
1108  }
1109 
1110  break;
1111  }
1112 
1113 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 14
1114 
1115  case PWM_SERVO_SET(13):
1116  case PWM_SERVO_SET(12):
1117  case PWM_SERVO_SET(11):
1118  case PWM_SERVO_SET(10):
1119  case PWM_SERVO_SET(9):
1120  case PWM_SERVO_SET(8):
1121  if (_mode < MODE_14PWM) {
1122  ret = -EINVAL;
1123  break;
1124  }
1125 
1126 #endif
1127 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8
1128 
1129  case PWM_SERVO_SET(7):
1130 
1131  /* FALLTHROUGH */
1132  case PWM_SERVO_SET(6):
1133  if (_mode < MODE_8PWM) {
1134  ret = -EINVAL;
1135  break;
1136  }
1137 
1138 #endif
1139 
1140 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6
1141 
1142  /* FALLTHROUGH */
1143  case PWM_SERVO_SET(5):
1144  if (_mode < MODE_6PWM) {
1145  ret = -EINVAL;
1146  break;
1147  }
1148 
1149 #endif
1150 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 5
1151 
1152  /* FALLTHROUGH */
1153  case PWM_SERVO_SET(4):
1154  if (_mode < MODE_5PWM) {
1155  ret = -EINVAL;
1156  break;
1157  }
1158 
1159 #endif
1160 
1161  /* FALLTHROUGH */
1162  case PWM_SERVO_SET(3):
1163  if (_mode < MODE_4PWM) {
1164  ret = -EINVAL;
1165  break;
1166  }
1167 
1168  /* FALLTHROUGH */
1169  case PWM_SERVO_SET(2):
1170  if (_mode < MODE_3PWM) {
1171  ret = -EINVAL;
1172  break;
1173  }
1174 
1175  /* FALLTHROUGH */
1176  case PWM_SERVO_SET(1):
1177  case PWM_SERVO_SET(0):
1178  if (arg <= 2100) {
1179  up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg);
1180 
1181  } else {
1182  ret = -EINVAL;
1183  }
1184 
1185  break;
1186 
1187 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 14
1188 
1189  case PWM_SERVO_GET(13):
1190  case PWM_SERVO_GET(12):
1191  case PWM_SERVO_GET(11):
1192  case PWM_SERVO_GET(10):
1193  case PWM_SERVO_GET(9):
1194  case PWM_SERVO_GET(8):
1195  if (_mode < MODE_14PWM) {
1196  ret = -EINVAL;
1197  break;
1198  }
1199 
1200 #endif
1201 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8
1202 
1203  /* FALLTHROUGH */
1204  case PWM_SERVO_GET(7):
1205  case PWM_SERVO_GET(6):
1206  if (_mode < MODE_8PWM) {
1207  ret = -EINVAL;
1208  break;
1209  }
1210 
1211 #endif
1212 
1213 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6
1214 
1215  /* FALLTHROUGH */
1216  case PWM_SERVO_GET(5):
1217  if (_mode < MODE_6PWM) {
1218  ret = -EINVAL;
1219  break;
1220  }
1221 
1222 #endif
1223 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 5
1224 
1225  /* FALLTHROUGH */
1226  case PWM_SERVO_GET(4):
1227  if (_mode < MODE_5PWM) {
1228  ret = -EINVAL;
1229  break;
1230  }
1231 
1232 #endif
1233 
1234  /* FALLTHROUGH */
1235  case PWM_SERVO_GET(3):
1236  if (_mode < MODE_4PWM) {
1237  ret = -EINVAL;
1238  break;
1239  }
1240 
1241  /* FALLTHROUGH */
1242  case PWM_SERVO_GET(2):
1243  if (_mode < MODE_3PWM) {
1244  ret = -EINVAL;
1245  break;
1246  }
1247 
1248  /* FALLTHROUGH */
1249  case PWM_SERVO_GET(1):
1250  case PWM_SERVO_GET(0):
1251  *(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0));
1252  break;
1253 
1254  case PWM_SERVO_GET_RATEGROUP(0):
1255  case PWM_SERVO_GET_RATEGROUP(1):
1256  case PWM_SERVO_GET_RATEGROUP(2):
1257  case PWM_SERVO_GET_RATEGROUP(3):
1258 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 5
1259  case PWM_SERVO_GET_RATEGROUP(4):
1260 #endif
1261 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6
1262  case PWM_SERVO_GET_RATEGROUP(5):
1263 #endif
1264 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8
1265  case PWM_SERVO_GET_RATEGROUP(6):
1266  case PWM_SERVO_GET_RATEGROUP(7):
1267 #endif
1268 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 14
1269  case PWM_SERVO_GET_RATEGROUP(8):
1270  case PWM_SERVO_GET_RATEGROUP(9):
1271  case PWM_SERVO_GET_RATEGROUP(10):
1272  case PWM_SERVO_GET_RATEGROUP(11):
1273  case PWM_SERVO_GET_RATEGROUP(12):
1274  case PWM_SERVO_GET_RATEGROUP(13):
1275 #endif
1276  *(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
1277  break;
1278 
1279  case PWM_SERVO_GET_COUNT:
1280  switch (_mode) {
1281 
1282 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 14
1283 
1284  case MODE_14PWM:
1285  *(unsigned *)arg = 14;
1286  break;
1287 #endif
1288 
1289 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8
1290 
1291  case MODE_8PWM:
1292  *(unsigned *)arg = 8;
1293  break;
1294 #endif
1295 
1296 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6
1297 
1298  case MODE_6PWM:
1299  *(unsigned *)arg = 6;
1300  break;
1301 #endif
1302 
1303  case MODE_5PWM:
1304  case MODE_5PWM1CAP:
1305  *(unsigned *)arg = 5;
1306  break;
1307 
1308  case MODE_4PWM:
1309  case MODE_4PWM1CAP:
1310  case MODE_4PWM2CAP:
1311  *(unsigned *)arg = 4;
1312  break;
1313 
1314  case MODE_3PWM:
1315  case MODE_3PWM1CAP:
1316  *(unsigned *)arg = 3;
1317  break;
1318 
1319  case MODE_2PWM:
1320  case MODE_2PWM2CAP:
1321  *(unsigned *)arg = 2;
1322  break;
1323 
1324  case MODE_1PWM:
1325  *(unsigned *)arg = 1;
1326  break;
1327 
1328  default:
1329  ret = -EINVAL;
1330  break;
1331  }
1332 
1333  break;
1334 
1335  case PWM_SERVO_SET_COUNT: {
1336  /* change the number of outputs that are enabled for
1337  * PWM. This is used to change the split between GPIO
1338  * and PWM under control of the flight config
1339  * parameters.
1340  */
1341  switch (arg) {
1342  case 0:
1344  break;
1345 
1346  case 1:
1348  break;
1349 
1350  case 2:
1352  break;
1353 
1354  case 3:
1356  break;
1357 
1358  case 4:
1360  break;
1361 
1362  case 5:
1364  break;
1365 
1366 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >=6
1367 
1368  case 6:
1370  break;
1371 #endif
1372 
1373 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >=8
1374 
1375  case 8:
1377  break;
1378 #endif
1379 
1380  default:
1381  ret = -EINVAL;
1382  break;
1383  }
1384 
1385  break;
1386  }
1387 
1388  case PWM_SERVO_SET_MODE: {
1389  switch (arg) {
1390  case PWM_SERVO_MODE_NONE:
1391  ret = set_mode(MODE_NONE);
1392  break;
1393 
1394  case PWM_SERVO_MODE_1PWM:
1395  ret = set_mode(MODE_1PWM);
1396  break;
1397 
1398  case PWM_SERVO_MODE_2PWM:
1399  ret = set_mode(MODE_2PWM);
1400  break;
1401 
1403  ret = set_mode(MODE_2PWM2CAP);
1404  break;
1405 
1406  case PWM_SERVO_MODE_3PWM:
1407  ret = set_mode(MODE_3PWM);
1408  break;
1409 
1411  ret = set_mode(MODE_3PWM1CAP);
1412  break;
1413 
1414  case PWM_SERVO_MODE_4PWM:
1415  ret = set_mode(MODE_4PWM);
1416  break;
1417 
1419  ret = set_mode(MODE_4PWM1CAP);
1420  break;
1421 
1423  ret = set_mode(MODE_4PWM2CAP);
1424  break;
1425 
1426  case PWM_SERVO_MODE_5PWM:
1427  ret = set_mode(MODE_5PWM);
1428  break;
1429 
1431  ret = set_mode(MODE_5PWM1CAP);
1432  break;
1433 
1434  case PWM_SERVO_MODE_6PWM:
1435  ret = set_mode(MODE_6PWM);
1436  break;
1437 
1438  case PWM_SERVO_MODE_8PWM:
1439  ret = set_mode(MODE_8PWM);
1440  break;
1441 
1442  case PWM_SERVO_MODE_4CAP:
1443  ret = set_mode(MODE_4CAP);
1444  break;
1445 
1446  case PWM_SERVO_MODE_5CAP:
1447  ret = set_mode(MODE_5CAP);
1448  break;
1449 
1450  case PWM_SERVO_MODE_6CAP:
1451  ret = set_mode(MODE_6CAP);
1452  break;
1453 
1455  _test_mode = true;
1456  break;
1457 
1459  _test_mode = false;
1460  break;
1461 
1462  default:
1463  ret = -EINVAL;
1464  }
1465 
1466  break;
1467  }
1468 
1469  case MIXERIOCRESET:
1471 
1472  break;
1473 
1474  case MIXERIOCLOADBUF: {
1475  const char *buf = (const char *)arg;
1476  unsigned buflen = strlen(buf);
1477  ret = _mixing_output.loadMixerThreadSafe(buf, buflen);
1478  update_pwm_trims();
1479 
1480  break;
1481  }
1482 
1483  default:
1484  ret = -ENOTTY;
1485  break;
1486  }
1487 
1488  unlock();
1489 
1490  return ret;
1491 }
1492 
1493 void
1495 {
1496  if (ms < 1) {
1497  ms = 1;
1498  }
1499 
1500  board_spi_reset(ms);
1501 }
1502 
1503 void
1505 {
1506  if (ms < 1) {
1507  ms = 10;
1508  }
1509 
1510  board_peripheral_reset(ms);
1511 }
1512 
1513 int
1514 PX4FMU::capture_ioctl(struct file *filp, int cmd, unsigned long arg)
1515 {
1516  int ret = -EINVAL;
1517 
1518 #if defined(BOARD_HAS_CAPTURE)
1519 
1520  lock();
1521 
1522  input_capture_config_t *pconfig = 0;
1523 
1525 
1526  if (_mode == MODE_3PWM1CAP || _mode == MODE_2PWM2CAP ||
1528  _mode == MODE_4PWM2CAP) {
1529 
1530  pconfig = (input_capture_config_t *)arg;
1531  }
1532 
1533  switch (cmd) {
1534 
1535  case INPUT_CAP_SET:
1536  if (pconfig) {
1537  ret = up_input_capture_set(pconfig->channel, pconfig->edge, pconfig->filter,
1538  pconfig->callback, pconfig->context);
1539  }
1540 
1541  break;
1542 
1544  if (pconfig) {
1545  ret = up_input_capture_set_callback(pconfig->channel, pconfig->callback, pconfig->context);
1546  }
1547 
1548  break;
1549 
1551  if (pconfig) {
1552  ret = up_input_capture_get_callback(pconfig->channel, &pconfig->callback, &pconfig->context);
1553  }
1554 
1555  break;
1556 
1557  case INPUT_CAP_GET_STATS:
1558  if (arg) {
1559  ret = up_input_capture_get_stats(stats->chan_in_edges_out, stats, false);
1560  }
1561 
1562  break;
1563 
1565  if (arg) {
1566  ret = up_input_capture_get_stats(stats->chan_in_edges_out, stats, true);
1567  }
1568 
1569  break;
1570 
1571  case INPUT_CAP_SET_EDGE:
1572  if (pconfig) {
1573  ret = up_input_capture_set_trigger(pconfig->channel, pconfig->edge);
1574  }
1575 
1576  break;
1577 
1578  case INPUT_CAP_GET_EDGE:
1579  if (pconfig) {
1580  ret = up_input_capture_get_trigger(pconfig->channel, &pconfig->edge);
1581  }
1582 
1583  break;
1584 
1585  case INPUT_CAP_SET_FILTER:
1586  if (pconfig) {
1587  ret = up_input_capture_set_filter(pconfig->channel, pconfig->filter);
1588  }
1589 
1590  break;
1591 
1592  case INPUT_CAP_GET_FILTER:
1593  if (pconfig) {
1594  ret = up_input_capture_get_filter(pconfig->channel, &pconfig->filter);
1595  }
1596 
1597  break;
1598 
1599  case INPUT_CAP_GET_COUNT:
1600  ret = OK;
1601 
1602  switch (_mode) {
1603  case MODE_5PWM1CAP:
1604  case MODE_4PWM1CAP:
1605  case MODE_3PWM1CAP:
1606  *(unsigned *)arg = 1;
1607  break;
1608 
1609  case MODE_2PWM2CAP:
1610  case MODE_4PWM2CAP:
1611  *(unsigned *)arg = 2;
1612  break;
1613 
1614  default:
1615  ret = -EINVAL;
1616  break;
1617  }
1618 
1619  break;
1620 
1621  case INPUT_CAP_SET_COUNT:
1622  ret = OK;
1623 
1624  switch (_mode) {
1625  case MODE_3PWM1CAP:
1627  break;
1628 
1629  case MODE_2PWM2CAP:
1631  break;
1632 
1633  case MODE_4PWM1CAP:
1635  break;
1636 
1637  case MODE_4PWM2CAP:
1639  break;
1640 
1641  case MODE_5PWM1CAP:
1643  break;
1644 
1645  default:
1646  ret = -EINVAL;
1647  break;
1648  }
1649 
1650  break;
1651 
1652  default:
1653  ret = -ENOTTY;
1654  break;
1655  }
1656 
1657  unlock();
1658 
1659 #else
1660  ret = -ENOTTY;
1661 #endif
1662  return ret;
1663 }
1664 
1665 int
1667 {
1668  if (!is_running()) {
1669  return -1;
1670  }
1671 
1672  PX4FMU::Mode servo_mode;
1673 
1674  servo_mode = PX4FMU::MODE_NONE;
1675 
1676  switch (new_mode) {
1677  case PORT_FULL_GPIO:
1678  case PORT_MODE_UNSET:
1679  break;
1680 
1681  case PORT_FULL_PWM:
1682 
1683 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 4
1684  /* select 4-pin PWM mode */
1685  servo_mode = PX4FMU::MODE_4PWM;
1686 #endif
1687 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 5
1688  servo_mode = PX4FMU::MODE_5PWM;
1689 #endif
1690 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 6
1691  servo_mode = PX4FMU::MODE_6PWM;
1692 #endif
1693 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 8
1694  servo_mode = PX4FMU::MODE_8PWM;
1695 #endif
1696 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 14
1697  servo_mode = PX4FMU::MODE_14PWM;
1698 #endif
1699  break;
1700 
1701  case PORT_PWM1:
1702  /* select 2-pin PWM mode */
1703  servo_mode = PX4FMU::MODE_1PWM;
1704  break;
1705 
1706 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8
1707 
1708  case PORT_PWM8:
1709  /* select 8-pin PWM mode */
1710  servo_mode = PX4FMU::MODE_8PWM;
1711  break;
1712 #endif
1713 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6
1714 
1715  case PORT_PWM6:
1716  /* select 6-pin PWM mode */
1717  servo_mode = PX4FMU::MODE_6PWM;
1718  break;
1719 
1720  case PORT_PWM5:
1721  /* select 5-pin PWM mode */
1722  servo_mode = PX4FMU::MODE_5PWM;
1723  break;
1724 
1725 
1726 # if defined(BOARD_HAS_CAPTURE)
1727 
1728  case PORT_PWM5CAP1:
1729  /* select 5-pin PWM mode 1 capture */
1730  servo_mode = PX4FMU::MODE_5PWM1CAP;
1731  break;
1732 
1733 # endif
1734 
1735  case PORT_PWM4:
1736  /* select 4-pin PWM mode */
1737  servo_mode = PX4FMU::MODE_4PWM;
1738  break;
1739 
1740 
1741 # if defined(BOARD_HAS_CAPTURE)
1742 
1743  case PORT_PWM4CAP1:
1744  /* select 4-pin PWM mode 1 capture */
1745  servo_mode = PX4FMU::MODE_4PWM1CAP;
1746  break;
1747 
1748  case PORT_PWM4CAP2:
1749  /* select 4-pin PWM mode 2 capture */
1750  servo_mode = PX4FMU::MODE_4PWM2CAP;
1751  break;
1752 
1753 # endif
1754 
1755  case PORT_PWM3:
1756  /* select 3-pin PWM mode */
1757  servo_mode = PX4FMU::MODE_3PWM;
1758  break;
1759 
1760 # if defined(BOARD_HAS_CAPTURE)
1761 
1762  case PORT_PWM3CAP1:
1763  /* select 3-pin PWM mode 1 capture */
1764  servo_mode = PX4FMU::MODE_3PWM1CAP;
1765  break;
1766 # endif
1767 
1768  case PORT_PWM2:
1769  /* select 2-pin PWM mode */
1770  servo_mode = PX4FMU::MODE_2PWM;
1771  break;
1772 
1773 # if defined(BOARD_HAS_CAPTURE)
1774 
1775  case PORT_PWM2CAP2:
1776  /* select 2-pin PWM mode 2 capture */
1777  servo_mode = PX4FMU::MODE_2PWM2CAP;
1778  break;
1779 
1780 # endif
1781 #endif
1782 
1783  default:
1784  return -1;
1785  }
1786 
1787  PX4FMU *object = get_instance();
1788 
1789  if (servo_mode != object->get_mode()) {
1790  /* (re)set the PWM output mode */
1791  object->set_mode(servo_mode);
1792  }
1793 
1794  return OK;
1795 }
1796 
1797 
1798 namespace
1799 {
1800 
1801 int fmu_new_i2c_speed(unsigned bus, unsigned clock_hz)
1802 {
1803  return PX4FMU::set_i2c_bus_clock(bus, clock_hz);
1804 }
1805 
1806 } // namespace
1807 
1808 int
1810 {
1811  int fd;
1812  unsigned servo_count = 0;
1813  unsigned capture_count = 0;
1814  unsigned pwm_value = 1000;
1815  int direction = 1;
1816  int ret;
1817  int rv = -1;
1818  uint32_t rate_limit = 0;
1819  struct input_capture_t {
1820  bool valid;
1822  } capture_conf[INPUT_CAPTURE_MAX_CHANNELS];
1823 
1824  fd = ::open(PX4FMU_DEVICE_PATH, O_RDWR);
1825 
1826  if (fd < 0) {
1827  PX4_ERR("open fail");
1828  return -1;
1829  }
1830 
1832  PX4_ERR("Failed to Enter pwm test mode");
1833  goto err_out_no_test;
1834  }
1835 
1836  if (::ioctl(fd, PWM_SERVO_ARM, 0) < 0) {
1837  PX4_ERR("servo arm failed");
1838  goto err_out;
1839  }
1840 
1841  if (::ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) {
1842  PX4_ERR("Unable to get servo count");
1843  goto err_out;
1844  }
1845 
1846  if (::ioctl(fd, INPUT_CAP_GET_COUNT, (unsigned long)&capture_count) != 0) {
1847  PX4_INFO("Not in a capture mode");
1848  }
1849 
1850  PX4_INFO("Testing %u servos and %u input captures", (unsigned)servo_count, capture_count);
1851  memset(capture_conf, 0, sizeof(capture_conf));
1852 
1853  if (capture_count != 0) {
1854  for (unsigned i = 0; i < capture_count; i++) {
1855  // Map to channel number
1856  capture_conf[i].chan.channel = i + servo_count;
1857 
1858  /* Save handler */
1859  if (::ioctl(fd, INPUT_CAP_GET_CALLBACK, (unsigned long)&capture_conf[i].chan.channel) != 0) {
1860  PX4_ERR("Unable to get capture callback for chan %u\n", capture_conf[i].chan.channel);
1861  goto err_out;
1862 
1863  } else {
1864  input_capture_config_t conf = capture_conf[i].chan;
1866  conf.context = PX4FMU::get_instance();
1867 
1868  if (::ioctl(fd, INPUT_CAP_SET_CALLBACK, (unsigned long)&conf) == 0) {
1869  capture_conf[i].valid = true;
1870 
1871  } else {
1872  PX4_ERR("Unable to set capture callback for chan %u\n", capture_conf[i].chan.channel);
1873  goto err_out;
1874  }
1875  }
1876 
1877  }
1878  }
1879 
1880  struct pollfd fds;
1881 
1882  fds.fd = 0; /* stdin */
1883 
1884  fds.events = POLLIN;
1885 
1886  PX4_INFO("Press CTRL-C or 'c' to abort.");
1887 
1888  for (;;) {
1889  /* sweep all servos between 1000..2000 */
1890  servo_position_t servos[servo_count];
1891 
1892  for (unsigned i = 0; i < servo_count; i++) {
1893  servos[i] = pwm_value;
1894  }
1895 
1896  for (unsigned i = 0; i < servo_count; i++) {
1897  if (::ioctl(fd, PWM_SERVO_SET(i), servos[i]) < 0) {
1898  PX4_ERR("servo %u set failed", i);
1899  goto err_out;
1900  }
1901  }
1902 
1903  if (direction > 0) {
1904  if (pwm_value < 2000) {
1905  pwm_value++;
1906 
1907  } else {
1908  direction = -1;
1909  }
1910 
1911  } else {
1912  if (pwm_value > 1000) {
1913  pwm_value--;
1914 
1915  } else {
1916  direction = 1;
1917  }
1918  }
1919 
1920  /* readback servo values */
1921  for (unsigned i = 0; i < servo_count; i++) {
1922  servo_position_t value;
1923 
1924  if (::ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value)) {
1925  PX4_ERR("error reading PWM servo %d", i);
1926  goto err_out;
1927  }
1928 
1929  if (value != servos[i]) {
1930  PX4_ERR("servo %d readback error, got %u expected %u", i, value, servos[i]);
1931  goto err_out;
1932  }
1933  }
1934 
1935  if (capture_count != 0 && (++rate_limit % 500 == 0)) {
1936  for (unsigned i = 0; i < capture_count; i++) {
1937  if (capture_conf[i].valid) {
1938  input_capture_stats_t stats;
1939  stats.chan_in_edges_out = capture_conf[i].chan.channel;
1940 
1941  if (::ioctl(fd, INPUT_CAP_GET_STATS, (unsigned long)&stats) != 0) {
1942  PX4_ERR("Unable to get stats for chan %u\n", capture_conf[i].chan.channel);
1943  goto err_out;
1944 
1945  } else {
1946  fprintf(stdout, "FMU: Status chan:%u edges: %d last time:%lld last state:%d overflows:%d lantency:%u\n",
1947  capture_conf[i].chan.channel,
1948  stats.chan_in_edges_out,
1949  stats.last_time,
1950  stats.last_edge,
1951  stats.overflows,
1952  stats.latnecy);
1953  }
1954  }
1955  }
1956 
1957  }
1958 
1959  /* Check if user wants to quit */
1960  char c;
1961  ret = ::poll(&fds, 1, 0);
1962 
1963  if (ret > 0) {
1964 
1965  ::read(0, &c, 1);
1966 
1967  if (c == 0x03 || c == 0x63 || c == 'q') {
1968  PX4_INFO("User abort");
1969  break;
1970  }
1971  }
1972  }
1973 
1974  if (capture_count != 0) {
1975  for (unsigned i = 0; i < capture_count; i++) {
1976  // Map to channel number
1977  if (capture_conf[i].valid) {
1978  /* Save handler */
1979  if (::ioctl(fd, INPUT_CAP_SET_CALLBACK, (unsigned long)&capture_conf[i].chan) != 0) {
1980  PX4_ERR("Unable to set capture callback for chan %u\n", capture_conf[i].chan.channel);
1981  goto err_out;
1982  }
1983  }
1984  }
1985  }
1986 
1987  rv = 0;
1988 
1989 err_out:
1990 
1992  PX4_ERR("Failed to Exit pwm test mode");
1993  }
1994 
1995 err_out_no_test:
1996  ::close(fd);
1997  return rv;
1998 }
1999 
2000 int
2001 PX4FMU::fake(int argc, char *argv[])
2002 {
2003  if (argc < 5) {
2004  print_usage("not enough arguments");
2005  return -1;
2006  }
2007 
2009 
2010  ac.control[0] = strtol(argv[1], 0, 0) / 100.0f;
2011 
2012  ac.control[1] = strtol(argv[2], 0, 0) / 100.0f;
2013 
2014  ac.control[2] = strtol(argv[3], 0, 0) / 100.0f;
2015 
2016  ac.control[3] = strtol(argv[4], 0, 0) / 100.0f;
2017 
2019 
2020  if (handle == nullptr) {
2021  PX4_ERR("advertise failed");
2022  return -1;
2023  }
2024 
2025  orb_unadvertise(handle);
2026 
2027  actuator_armed_s aa;
2028 
2029  aa.armed = true;
2030  aa.lockdown = false;
2031 
2032  handle = orb_advertise(ORB_ID(actuator_armed), &aa);
2033 
2034  if (handle == nullptr) {
2035  PX4_ERR("advertise failed 2");
2036  return -1;
2037  }
2038 
2039  orb_unadvertise(handle);
2040  return 0;
2041 }
2042 
2043 int PX4FMU::custom_command(int argc, char *argv[])
2044 {
2045  PortMode new_mode = PORT_MODE_UNSET;
2046  const char *verb = argv[0];
2047 
2048  /* does not operate on a FMU instance */
2049  if (!strcmp(verb, "i2c")) {
2050  if (argc > 2) {
2051  int bus = strtol(argv[1], 0, 0);
2052  int clock_hz = strtol(argv[2], 0, 0);
2053  int ret = fmu_new_i2c_speed(bus, clock_hz);
2054 
2055  if (ret) {
2056  PX4_ERR("setting I2C clock failed");
2057  }
2058 
2059  return ret;
2060  }
2061 
2062  return print_usage("not enough arguments");
2063  }
2064 
2065  if (!strcmp(verb, "sensor_reset")) {
2066  if (argc > 1) {
2067  int reset_time = strtol(argv[1], nullptr, 0);
2068  sensor_reset(reset_time);
2069 
2070  } else {
2071  sensor_reset(0);
2072  PX4_INFO("reset default time");
2073  }
2074 
2075  return 0;
2076  }
2077 
2078  if (!strcmp(verb, "peripheral_reset")) {
2079  if (argc > 2) {
2080  int reset_time = strtol(argv[2], 0, 0);
2081  peripheral_reset(reset_time);
2082 
2083  } else {
2084  peripheral_reset(0);
2085  PX4_INFO("reset default time");
2086  }
2087 
2088  return 0;
2089  }
2090 
2091 
2092  /* start the FMU if not running */
2093  if (!is_running()) {
2094  int ret = PX4FMU::task_spawn(argc, argv);
2095 
2096  if (ret) {
2097  return ret;
2098  }
2099  }
2100 
2101  /*
2102  * Mode switches.
2103  */
2104  if (!strcmp(verb, "mode_gpio")) {
2105  new_mode = PORT_FULL_GPIO;
2106 
2107  } else if (!strcmp(verb, "mode_pwm")) {
2108  new_mode = PORT_FULL_PWM;
2109 
2110  // mode: defines which outputs to drive (others may be used by other tasks such as camera capture)
2111 #if defined(BOARD_HAS_PWM)
2112 
2113  } else if (!strcmp(verb, "mode_pwm1")) {
2114  new_mode = PORT_PWM1;
2115 #endif
2116 
2117 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6
2118 
2119  } else if (!strcmp(verb, "mode_pwm6")) {
2120  new_mode = PORT_PWM6;
2121 
2122 #endif
2123 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 5
2124 
2125  } else if (!strcmp(verb, "mode_pwm5")) {
2126  new_mode = PORT_PWM5;
2127 
2128 # if defined(BOARD_HAS_CAPTURE)
2129 
2130  } else if (!strcmp(verb, "mode_pwm5cap1")) {
2131  new_mode = PORT_PWM5CAP1;
2132 # endif
2133 
2134  } else if (!strcmp(verb, "mode_pwm4")) {
2135  new_mode = PORT_PWM4;
2136 
2137 # if defined(BOARD_HAS_CAPTURE)
2138 
2139  } else if (!strcmp(verb, "mode_pwm4cap1")) {
2140  new_mode = PORT_PWM4CAP1;
2141 
2142  } else if (!strcmp(verb, "mode_pwm4cap2")) {
2143  new_mode = PORT_PWM4CAP2;
2144 # endif
2145 
2146  } else if (!strcmp(verb, "mode_pwm3")) {
2147  new_mode = PORT_PWM3;
2148 
2149 # if defined(BOARD_HAS_CAPTURE)
2150 
2151  } else if (!strcmp(verb, "mode_pwm3cap1")) {
2152  new_mode = PORT_PWM3CAP1;
2153 # endif
2154 
2155  } else if (!strcmp(verb, "mode_pwm2")) {
2156  new_mode = PORT_PWM2;
2157 
2158 # if defined(BOARD_HAS_CAPTURE)
2159 
2160  } else if (!strcmp(verb, "mode_pwm2cap2")) {
2161  new_mode = PORT_PWM2CAP2;
2162 # endif
2163 #endif
2164 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8
2165 
2166  } else if (!strcmp(verb, "mode_pwm8")) {
2167  new_mode = PORT_PWM8;
2168 #endif
2169  }
2170 
2171  /* was a new mode set? */
2172  if (new_mode != PORT_MODE_UNSET) {
2173 
2174  /* switch modes */
2175  return PX4FMU::fmu_new_mode(new_mode);
2176  }
2177 
2178  if (!strcmp(verb, "test")) {
2179  return test();
2180  }
2181 
2182  if (!strcmp(verb, "fake")) {
2183  return fake(argc - 1, argv + 1);
2184  }
2185 
2186  return print_usage("unknown command");
2187 }
2188 
2190 {
2191  PX4_INFO("Max update rate: %i Hz", _current_update_rate);
2192 
2193  const char *mode_str = nullptr;
2194 
2195  switch (_mode) {
2196  case MODE_NONE: mode_str = "no pwm"; break;
2197 
2198  case MODE_1PWM: mode_str = "pwm1"; break;
2199 
2200  case MODE_2PWM: mode_str = "pwm2"; break;
2201 
2202  case MODE_2PWM2CAP: mode_str = "pwm2cap2"; break;
2203 
2204  case MODE_3PWM: mode_str = "pwm3"; break;
2205 
2206  case MODE_3PWM1CAP: mode_str = "pwm3cap1"; break;
2207 
2208  case MODE_4PWM: mode_str = "pwm4"; break;
2209 
2210  case MODE_4PWM1CAP: mode_str = "pwm4cap1"; break;
2211 
2212  case MODE_4PWM2CAP: mode_str = "pwm4cap2"; break;
2213 
2214  case MODE_5PWM: mode_str = "pwm5"; break;
2215 
2216  case MODE_5PWM1CAP: mode_str = "pwm5cap1"; break;
2217 
2218  case MODE_6PWM: mode_str = "pwm6"; break;
2219 
2220  case MODE_8PWM: mode_str = "pwm8"; break;
2221 
2222  case MODE_4CAP: mode_str = "cap4"; break;
2223 
2224  case MODE_5CAP: mode_str = "cap5"; break;
2225 
2226  case MODE_6CAP: mode_str = "cap6"; break;
2227 
2228  default:
2229  break;
2230  }
2231 
2232  if (mode_str) {
2233  PX4_INFO("PWM Mode: %s", mode_str);
2234  }
2235 
2238 
2239  return 0;
2240 }
2241 
2242 int PX4FMU::print_usage(const char *reason)
2243 {
2244  if (reason) {
2245  PX4_WARN("%s\n", reason);
2246  }
2247 
2248  PRINT_MODULE_DESCRIPTION(
2249  R"DESCR_STR(
2250 ### Description
2251 This module is responsible for driving the output and reading the input pins. For boards without a separate IO chip
2252 (eg. Pixracer), it uses the main channels. On boards with an IO chip (eg. Pixhawk), it uses the AUX channels, and the
2253 px4io driver is used for main ones.
2254 
2255 It listens on the actuator_controls topics, does the mixing and writes the PWM outputs.
2256 
2257 The module is configured via mode_* commands. This defines which of the first N pins the driver should occupy.
2258 By using mode_pwm4 for example, pins 5 and 6 can be used by the camera trigger driver or by a PWM rangefinder
2259 driver. Alternatively, the fmu can be started in one of the capture modes, and then drivers can register a capture
2260 callback with ioctl calls.
2261 
2262 ### Implementation
2263 By default the module runs on a work queue with a callback on the uORB actuator_controls topic.
2264 
2265 ### Examples
2266 It is typically started with:
2267 $ fmu mode_pwm
2268 To drive all available pins.
2269 
2270 Capture input (rising and falling edges) and print on the console: start the fmu in one of the capture modes:
2271 $ fmu mode_pwm3cap1
2272 This will enable capturing on the 4th pin. Then do:
2273 $ fmu test
2274 
2275 Use the `pwm` command for further configurations (PWM rate, levels, ...), and the `mixer` command to load
2276 mixer files.
2277 )DESCR_STR");
2278 
2279  PRINT_MODULE_USAGE_NAME("fmu", "driver");
2280  PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the task (without any mode set, use any of the mode_* cmds)");
2281 
2282  PRINT_MODULE_USAGE_PARAM_COMMENT("All of the mode_* commands will start the fmu if not running already");
2283 
2284  PRINT_MODULE_USAGE_COMMAND("mode_gpio");
2285  PRINT_MODULE_USAGE_COMMAND_DESCR("mode_pwm", "Select all available pins as PWM");
2286 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8
2287  PRINT_MODULE_USAGE_COMMAND("mode_pwm8");
2288 #endif
2289 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6
2290  PRINT_MODULE_USAGE_COMMAND("mode_pwm6");
2291  PRINT_MODULE_USAGE_COMMAND("mode_pwm5");
2292  PRINT_MODULE_USAGE_COMMAND("mode_pwm5cap1");
2293  PRINT_MODULE_USAGE_COMMAND("mode_pwm4");
2294  PRINT_MODULE_USAGE_COMMAND("mode_pwm4cap1");
2295  PRINT_MODULE_USAGE_COMMAND("mode_pwm4cap2");
2296  PRINT_MODULE_USAGE_COMMAND("mode_pwm3");
2297  PRINT_MODULE_USAGE_COMMAND("mode_pwm3cap1");
2298  PRINT_MODULE_USAGE_COMMAND("mode_pwm2");
2299  PRINT_MODULE_USAGE_COMMAND("mode_pwm2cap2");
2300 #endif
2301 #if defined(BOARD_HAS_PWM)
2302  PRINT_MODULE_USAGE_COMMAND("mode_pwm1");
2303 #endif
2304 
2305  PRINT_MODULE_USAGE_COMMAND_DESCR("sensor_reset", "Do a sensor reset (SPI bus)");
2306  PRINT_MODULE_USAGE_ARG("<ms>", "Delay time in ms between reset and re-enabling", true);
2307  PRINT_MODULE_USAGE_COMMAND_DESCR("peripheral_reset", "Reset board peripherals");
2308  PRINT_MODULE_USAGE_ARG("<ms>", "Delay time in ms between reset and re-enabling", true);
2309 
2310  PRINT_MODULE_USAGE_COMMAND_DESCR("i2c", "Configure I2C clock rate");
2311  PRINT_MODULE_USAGE_ARG("<bus_id> <rate>", "Specify the bus id (>=0) and rate in Hz", false);
2312 
2313  PRINT_MODULE_USAGE_COMMAND_DESCR("test", "Test inputs and outputs");
2314  PRINT_MODULE_USAGE_COMMAND_DESCR("fake", "Arm and send an actuator controls command");
2315  PRINT_MODULE_USAGE_ARG("<roll> <pitch> <yaw> <thrust>", "Control values in range [-100, 100]", false);
2316  PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
2317 
2318  return 0;
2319 }
2320 
2321 extern "C" __EXPORT int fmu_main(int argc, char *argv[])
2322 {
2323  return PX4FMU::main(argc, argv);
2324 }
#define PWM_DEFAULT_MAX
Default maximum PWM in us.
#define PARAM_INVALID
Handle returned when a parameter cannot be found.
Definition: param.h:103
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
#define PWM_SERVO_GET_SELECT_UPDATE_RATE
check the selected update rates
#define PWM_LOWEST_MAX
Lowest PWM allowed as the maximum PWM.
#define MIXERIOCLOADBUF
Add mixer(s) from the buffer in (const char *)arg.
Definition: drv_mixer.h:79
__EXPORT servo_position_t up_pwm_servo_get(unsigned channel)
Get the current output value for a channel.
#define PWM_SERVO_SET_ARM_OK
set the &#39;ARM ok&#39; bit, which activates the safety switch
const actuator_armed_s & armed() const
bool _pwm_on
Definition: fmu.cpp:182
bool _pwm_initialized
Definition: fmu.cpp:184
void printStatus() const
bool _test_mode
Definition: fmu.cpp:185
virtual int open(file_t *filep)
Handle an open of the device.
Definition: CDev.cpp:141
#define PWM_SERVO_MODE_2PWM
uint16_t servo_position_t
Servo output signal type, value is actual servo output pulse width in microseconds.
static int task_spawn(int argc, char *argv[])
Definition: fmu.cpp:668
measure the time elapsed performing an event
Definition: perf_counter.h:56
__EXPORT uint32_t up_pwm_servo_get_rate_group(unsigned group)
Get a bitmap of output channels assigned to a given rate group.
#define PWM_SERVO_SET_FORCE_SAFETY_ON
force safety switch on (to enable use of safety switch)
#define PWM_SERVO_MODE_4PWM
static int set_i2c_bus_clock(unsigned bus, unsigned clock_hz)
Definition: fmu.cpp:561
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
Definition: parameters.cpp:589
#define PWM_SERVO_GET_COUNT
get the number of servos in *(unsigned *)arg
#define PWM_SERVO_ARM
arm all servo outputs handle by this driver
#define INPUT_CAP_GET_STATS
Get channel stats - arg is pointer to input_capture_config with channel set.
#define PWM_SERVO_MODE_8PWM
#define PWM_SERVO_MODE_5PWM1CAP
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
uint16_t & reverseOutputMask()
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
static void sensor_reset(int ms)
Definition: fmu.cpp:1494
__EXPORT void up_pwm_servo_deinit(void)
De-initialise the PWM servo outputs.
Definition: I2C.hpp:51
static Mode _mode
Definition: motor_ramp.cpp:81
void setMaxTopicUpdateRate(unsigned max_topic_update_interval_us)
__EXPORT int up_input_capture_set_trigger(unsigned channel, input_capture_edge edge)
orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data)
Definition: uORB.cpp:43
#define PWM_SERVO_GET_UPDATE_RATE
get alternate servo update rate
#define PWM_SERVO_GET_DISARMED_PWM
get the PWM value when disarmed
void print_status()
Definition: Commander.cpp:517
static void capture_trampoline(void *context, uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow)
Definition: fmu.cpp:692
#define PWM_SERVO_MODE_NONE
set auxillary output mode.
void update_pwm_trims()
Definition: fmu.cpp:624
bool update()
Call this regularly from Run().
#define PWM_SERVO_CLEAR_ARM_OK
clear the &#39;ARM ok&#39; bit, which deactivates the safety switch
int set_mode(Mode mode)
Definition: fmu.cpp:267
static void print_usage()
uORB::Subscription _parameter_update_sub
Definition: fmu.cpp:176
static bool is_running()
Definition: dataman.cpp:415
#define PWM_SERVO_MODE_4PWM1CAP
LidarLite * instance
Definition: ll40ls.cpp:65
bool updateSubscriptions(bool allow_wq_switch)
Check for subscription updates (e.g.
#define PWM_SERVO_SET_SELECT_UPDATE_RATE
selects servo update rates, one bit per servo.
virtual ssize_t read(file_t *filep, char *buffer, size_t buflen)
Perform a read from the device.
Definition: CDev.hpp:111
#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.
unsigned set_trims(int16_t *v, unsigned n)
Definition: MixerGroup.cpp:75
Mode
Definition: fmu.cpp:101
virtual int close(file_t *filep)
Handle a close of the device.
Definition: CDev.cpp:166
static constexpr int MAX_ACTUATORS
Global flash based parameter store.
unsigned _pwm_alt_rate
Definition: fmu.cpp:171
__EXPORT int fmu_main(int argc, char *argv[])
Definition: fmu.cpp:2321
#define PWM_SERVO_SET(_servo)
set a single servo to a specific value
perf_counter_t _cycle_perf
Definition: fmu.cpp:189
Mixer ioctl interfaces.
__EXPORT int up_input_capture_get_trigger(unsigned channel, input_capture_edge *edge)
bool in_esc_calibration_mode
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
void update_current_rate()
Definition: fmu.cpp:567
#define PWM_SERVO_MODE_6PWM
#define PWM_SERVO_MODE_3PWM
Definition: fmu.cpp:98
#define PWM_SERVO_GET_DEFAULT_UPDATE_RATE
get default servo update rate
#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)
#define PWM_SERVO_SET_FAILSAFE_PWM
set the PWM value for failsafe
Abstract class for any character device.
Definition: CDev.hpp:58
void update_params()
Definition: fmu.cpp:786
#define INPUT_CAP_GET_FILTER
Set Filter input filter channel arg is pointer to input_capture_config with channel set...
uint16_t values[PWM_OUTPUT_MAX_CHANNELS]
Drive scheduling based on subscribed actuator controls topics (via uORB callbacks) ...
uint16_t & maxValue(int index)
Header common to all counters.
void perf_free(perf_counter_t handle)
Free a counter.
Mode get_mode()
Definition: fmu.cpp:149
void init()
Activates/configures the hardware registers.
#define PWM_SERVO_SET_DISARMED_PWM
set the PWM value when disarmed - should be no PWM (zero) by default
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
#define PWM_SERVO_MODE_5PWM
unsigned _num_disarmed_set
Definition: fmu.cpp:187
int _class_instance
Definition: fmu.cpp:180
#define perf_alloc(a, b)
Definition: px4io.h:59
#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS
Definition: uORB.h:256
#define INPUT_CAP_GET_CLR_STATS
Get channel stats - arg is pointer to input_capture_config with channel set.
#define PWM_SERVO_MODE_5CAP
__EXPORT int up_input_capture_get_filter(unsigned channel, capture_filter_t *filter)
void capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow)
Definition: fmu.cpp:700
__EXPORT int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
Set the update rate for a given rate group.
#define PWM_SERVO_SET_TRIM_PWM
set the TRIM value the output will send
#define PWM_SERVO_ENTER_TEST_MODE
virtual int init()
Definition: fmu.cpp:233
#define PWM_SERVO_MODE_4CAP
uint32_t _pwm_mask
Definition: fmu.cpp:183
void resetMixerThreadSafe()
Reset (unload) the complete mixer, called from another thread.
uint32_t channel_count
#define PWM_SERVO_GET_TRIM_PWM
get the TRIM value the output will send
#define INPUT_CAP_SET_EDGE
Set Edge a channel arg is pointer to input_capture_config with channel and edge set.
PX4FMU()
Definition: fmu.cpp:211
__EXPORT int up_pwm_servo_set(unsigned channel, servo_position_t value)
Set the current output value for a channel.
#define PWM_SERVO_SET_FORCE_SAFETY_OFF
force safety switch off (to disable use of safety switch)
PortMode
Mode given via CLI.
Definition: dshot.cpp:72
int pwm_ioctl(file *filp, int cmd, unsigned long arg)
Definition: fmu.cpp:844
void perf_end(perf_counter_t handle)
End a performance event.
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: fmu.cpp:719
#define PWM_SERVO_MODE_6CAP
void test(enum LPS25H_BUS busid)
Perform some basic functional tests on the driver; make sure we can collect data from the sensor in p...
Definition: lps25h.cpp:792
virtual ~PX4FMU()
Definition: fmu.cpp:221
__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...
#define PWM_SERVO_GET_RATEGROUP(_n)
get the _n&#39;th rate group&#39;s channels; *(uint32_t *)arg returns a bitmap of channels whose update rates...
__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.
#define PWM_SERVO_GET(_servo)
get a single specific servo value
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
int fd
Definition: dataman.cpp:146
#define PWM_SERVO_SET_MIN_PWM
set the minimum PWM value the output will send
#define PWM_SERVO_MODE_4PWM2CAP
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
Definition: parameters.cpp:370
#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
unsigned _current_update_rate
Definition: fmu.cpp:174
void setAllMaxValues(uint16_t value)
void Run() override
Definition: fmu.cpp:744
#define PWM_SERVO_SET_MAX_PWM
set the maximum PWM value the output will send
input capture values for a channel
#define PWM_SERVO_SET_MODE
static int fake(int argc, char *argv[])
Definition: fmu.cpp:2001
uint16_t & disarmedValue(int index)
Disarmed values: disarmedValue < minValue needs to hold.
#define PWM_SERVO_GET_MIN_PWM
get the minimum PWM value the output will send
#define PWM_OUTPUT_BASE_DEVICE_PATH
Path for the default PWM output device.
#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
static int print_usage(const char *reason=nullptr)
Definition: fmu.cpp:2242
__EXPORT int up_input_capture_get_stats(unsigned channel, input_capture_stats_t *stats, bool clear)
#define PWM_HIGHEST_MIN
Highest PWM allowed as the minimum PWM.
__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.
static int test()
Definition: fmu.cpp:1809
void update_pwm_out_state(bool on)
Definition: fmu.cpp:707
void setDriverInstance(uint8_t instance)
__EXPORT int up_pwm_servo_init(uint32_t channel_mask)
Intialise the PWM servo outputs using the specified configuration.
static int custom_command(int argc, char *argv[])
Definition: fmu.cpp:2043
int orb_unadvertise(orb_advert_t handle)
Definition: uORB.cpp:65
Definition: bst.cpp:62
MixingOutput _mixing_output
Definition: fmu.cpp:166
__EXPORT void up_pwm_servo_arm(bool armed)
Arm or disarm servo outputs.
uint16_t & minValue(int index)
#define OK
Definition: uavcan_main.cpp:71
int capture_ioctl(file *filp, int cmd, unsigned long arg)
Definition: fmu.cpp:1514
static void peripheral_reset(int ms)
Definition: fmu.cpp:1504
#define PWM_HIGHEST_MAX
Highest maximum PWM in us.
MixerGroup * mixers() const
int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate)
Definition: fmu.cpp:493
void update_pwm_rev_mask()
Definition: fmu.cpp:590
uint32_t _pwm_alt_rate_channels
Definition: fmu.cpp:172
void unregister()
unregister uORB subscription callbacks
#define PX4FMU_DEVICE_PATH
Definition: fmu.cpp:95
#define PWM_SERVO_MODE_1PWM
static constexpr int FMU_MAX_ACTUATORS
Definition: fmu.cpp:163
unsigned _num_outputs
Definition: fmu.cpp:179
Mode _mode
Definition: fmu.cpp:168
static int fmu_new_mode(PortMode new_mode)
change the FMU mode of the running module
Definition: fmu.cpp:1666
void unlock()
Release the driver lock.
Definition: CDev.hpp:269
#define PWM_SERVO_GET_FAILSAFE_PWM
get the PWM value for failsafe
#define PWM_DEFAULT_MIN
Default minimum PWM in us.
mode
Definition: vtol_type.h:76
bool copy(void *dst)
Copy the struct.
__EXPORT void up_pwm_update(void)
Trigger all timer&#39;s channels in Oneshot mode to fire the oneshot with updated values.
virtual int ioctl(file *filp, int cmd, unsigned long arg)
Definition: fmu.cpp:795
int print_status() override
Definition: fmu.cpp:2189
#define PWM_SERVO_MODE_3PWM1CAP
#define INPUT_CAPTURE_MAX_CHANNELS
Maximum number of PWM input channels supported by the device.
void perf_begin(perf_counter_t handle)
Begin a performance event.
This handles the mixing, arming/disarming and all subscriptions required for that.
unsigned get_trims(int16_t *values)
Definition: MixerGroup.cpp:102
#define PWM_SERVO_SET_COUNT
set the number of servos in (unsigned)arg - allows change of split between servos and GPIO ...
uint32_t param_t
Parameter handle.
Definition: param.h:98
#define PWM_LOWEST_MIN
Lowest minimum PWM in us.
virtual int poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup)
Perform a poll setup/teardown operation.
Definition: CDev.cpp:213
#define PWM_SERVO_DISARM
disarm all servo outputs (stop generating pulses)
Performance measuring tools.
unsigned _pwm_default_rate
Definition: fmu.cpp:170
uint16_t & failsafeValue(int index)
#define PWM_SERVO_GET_MAX_PWM
get the maximum PWM value the output will send
#define PWM_SERVO_SET_UPDATE_RATE
set alternate servo update rate
Base class for devices connected via I2C.
void setAllMinValues(uint16_t value)
#define PWM_SERVO_EXIT_TEST_MODE