PX4 Firmware
PX4 Autopilot Software http://px4.io
RCInput.hpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 #pragma once
35 
36 #include <float.h>
37 
38 #include <board_config.h>
39 #include <drivers/drv_hrt.h>
40 #include <drivers/drv_rc_input.h>
41 #include <lib/perf/perf_counter.h>
42 #include <lib/rc/crsf.h>
43 #include <lib/rc/dsm.h>
44 #include <lib/rc/sbus.h>
45 #include <lib/rc/st24.h>
46 #include <lib/rc/sumd.h>
47 #include <px4_platform_common/px4_config.h>
48 #include <px4_platform_common/getopt.h>
49 #include <px4_platform_common/log.h>
50 #include <px4_platform_common/module.h>
51 #include <px4_platform_common/workqueue.h>
52 #include <uORB/Subscription.hpp>
54 #include <uORB/topics/adc_report.h>
55 #include <uORB/topics/input_rc.h>
57 
58 #include "crsf_telemetry.h"
59 
60 #ifdef HRT_PPM_CHANNEL
61 # include <systemlib/ppm_decode.h>
62 #endif
63 
64 class RCInput : public ModuleBase<RCInput>
65 {
66 public:
67 
68  RCInput(bool run_as_task, char *device);
69  virtual ~RCInput();
70 
71  /** @see ModuleBase */
72  static int task_spawn(int argc, char *argv[]);
73 
74  /** @see ModuleBase */
75  static RCInput *instantiate(int argc, char *argv[]);
76 
77  /** @see ModuleBase */
78  static int custom_command(int argc, char *argv[]);
79 
80  /** @see ModuleBase */
81  static int print_usage(const char *reason = nullptr);
82 
83  /** @see ModuleBase::run() */
84  void run() override;
85 
86  /**
87  * run the main loop: if running as task, continuously iterate, otherwise execute only one single cycle
88  */
89  void cycle();
90 
91  /** @see ModuleBase::print_status() */
92  int print_status() override;
93 
94  int init();
95 
96 private:
97  enum RC_SCAN {
104  } _rc_scan_state{RC_SCAN_SBUS};
105 
106  static constexpr char const *RC_SCAN_STRING[6] {
107  "PPM",
108  "SBUS",
109  "DSM",
110  "SUMD",
111  "ST24",
112  "CRSF"
113  };
114 
116 
117  bool _rc_scan_locked{false};
118  bool _report_lock{true};
119 
120  unsigned _current_update_interval{4000};
121 
122  bool _run_as_task{false};
123 
124  static struct work_s _work;
125 
128 
130 
131  float _analog_rc_rssi_volt{-1.0f};
133 
135 
136  int _rcs_fd{-1};
137  char _device[20] {}; ///< device / serial port path
138 
140 
141  uint16_t _raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {};
142  uint16_t _raw_rc_count{};
143 
145 
148 
149  static void cycle_trampoline(void *arg);
150  static void cycle_trampoline_init(void *arg);
151  int start();
152 
153  void fill_rc_in(uint16_t raw_rc_count_local,
154  uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS],
155  hrt_abstime now, bool frame_drop, bool failsafe,
156  unsigned frame_drops, int rssi);
157 
158  void set_rc_scan_state(RC_SCAN _rc_scan_state);
159 
160  void rc_io_invert(bool invert);
161 
162 };
static RCInput * instantiate(int argc, char *argv[])
Definition: RCInput.cpp:741
bool _report_lock
Definition: RCInput.hpp:118
uint16_t _raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS]
Definition: RCInput.hpp:141
R/C input interface.
CRSFTelemetry * _crsf_telemetry
Definition: RCInput.hpp:144
RC protocol definition for Spektrum RC.
hrt_abstime _rc_scan_begin
Definition: RCInput.hpp:115
RC protocol definition for Graupner HoTT transmitter.
static void cycle_trampoline(void *arg)
Definition: RCInput.cpp:215
virtual ~RCInput()
Definition: RCInput.cpp:72
void fill_rc_in(uint16_t raw_rc_count_local, uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS], hrt_abstime now, bool frame_drop, bool failsafe, unsigned frame_drops, int rssi)
Definition: RCInput.cpp:222
void rc_io_invert(bool invert)
Namespace encapsulating all device framework classes, functions and data.
Definition: CDev.cpp:47
High-resolution timer with callouts and timekeeping.
bool _rc_scan_locked
Definition: RCInput.hpp:117
enum RCInput::RC_SCAN RC_SCAN_SBUS
static int custom_command(int argc, char *argv[])
Definition: RCInput.cpp:747
uORB::Subscription _adc_sub
Definition: RCInput.hpp:127
static void cycle_trampoline_init(void *arg)
Definition: RCInput.cpp:193
int _rcs_fd
Definition: RCInput.hpp:136
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
RC protocol definition for Yuneec ST24 transmitter.
void run() override
Definition: RCInput.cpp:305
Header common to all counters.
unsigned _current_update_interval
Definition: RCInput.hpp:120
float _analog_rc_rssi_volt
Definition: RCInput.hpp:131
RCInput(bool run_as_task, char *device)
Definition: RCInput.cpp:47
High-level class that handles sending of CRSF telemetry data.
static constexpr char const * RC_SCAN_STRING[6]
Definition: RCInput.hpp:106
bool _analog_rc_rssi_stable
Definition: RCInput.hpp:132
uORB::Subscription _vehicle_cmd_sub
Definition: RCInput.hpp:126
#define SBUS_BUFFER_SIZE
Definition: sbus.h:50
static int task_spawn(int argc, char *argv[])
Definition: RCInput.cpp:121
uORB::PublicationMulti< input_rc_s > _to_input_rc
Definition: RCInput.hpp:134
perf_counter_t _publish_interval_perf
Definition: RCInput.hpp:147
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
int init()
Definition: RCInput.cpp:87
uint16_t _raw_rc_count
Definition: RCInput.hpp:142
uint8_t _rcs_buf[SBUS_BUFFER_SIZE]
Definition: RCInput.hpp:139
char _device[20]
device / serial port path
Definition: RCInput.hpp:137
RC protocol definition for S.BUS.
int print_status() override
Definition: RCInput.cpp:771
int start()
void set_rc_scan_state(RC_SCAN _rc_scan_state)
bool _run_as_task
Definition: RCInput.hpp:122
RC protocol definition for CSRF (TBS Crossfire).
input_rc_s _rc_in
Definition: RCInput.hpp:129
void cycle()
run the main loop: if running as task, continuously iterate, otherwise execute only one single cycle ...
Definition: RCInput.cpp:319
perf_counter_t _cycle_perf
Definition: RCInput.hpp:146
static int print_usage(const char *reason=nullptr)
Definition: RCInput.cpp:802
queue struct.
Performance measuring tools.
PPM input decoder.