PX4 Firmware
PX4 Autopilot Software http://px4.io
rpi_rc_in.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2017 Fan.zhang. All rights reserved. 421395590@qq.com
4  * Copyright (c) 2017 PX4 Development Team. All rights reserved.
5  *
6  * Redistribution and use in source and binary forms, with or without
7  * modification, are permitted provided that the following conditions
8  * are met:
9  *
10  * 1. Redistributions of source code must retain the above copyright
11  * notice, this list of conditions and the following disclaimer.
12  * 2. Redistributions in binary form must reproduce the above copyright
13  * notice, this list of conditions and the following disclaimer in
14  * the documentation and/or other materials provided with the
15  * distribution.
16  * 3. Neither the name PX4 nor the names of its contributors may be
17  * used to endorse or promote products derived from this software
18  * without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
27  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
28  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  *
33  ****************************************************************************/
34 #include "rpi_rc_in.h"
35 
36 using namespace rpi_rc_in;
37 
39 {
40  if (_mem) {
41  shmdt(_mem);
42  _mem = nullptr;
43  }
44 
45  ScheduleClear();
46  _is_running = false;
47 }
48 
50 {
51  int i;
52 
53  // initialize shared memory
54  if ((_shmid = shmget(_key, sizeof(int) * _channels, 0666)) < 0) {
55  PX4_WARN("Faild to access shared memory");
56  return -1;
57  }
58 
59  if ((_mem = (int *) shmat(_shmid, NULL, 0)) == (void *) - 1) {
60  PX4_WARN("Faild to map shared memory");
61  return -1;
62  }
63 
64  // publish data for all channels
65  for (i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; ++i) {
66  _data.values[i] = UINT16_MAX;
67  }
68 
69  return 0;
70 }
71 
73 {
74  int result = 0;
75 
76  result = rpi_rc_init();
77 
78  if (result != 0) {
79  PX4_WARN("error: RC initialization failed");
80  return -1;
81  }
82 
83  _is_running = true;
84 
85  ScheduleNow();
86 
87  return result;
88 }
89 
91 {
92  _should_exit = true;
93 }
94 
96 {
97  _measure();
98 
99  if (!_should_exit) {
100  ScheduleDelayed(RCINPUT_MEASURE_INTERVAL_US);
101  }
102 }
103 
105 {
106  uint64_t ts;
107  // publish PWM data
108  // read pwm value from shared memory
109  int i = 0;
110 
111  for (i = 0; i < _channels; ++i) {
112  int value = _mem[i]; // access the shared memory (with a single read)
113  _data.values[i] = (value <= 0) ? UINT16_MAX : value;
114  }
115 
116  ts = hrt_absolute_time();
117  _data.timestamp = ts;
120  _data.rssi = 100;
124  _data.rc_failsafe = false;
125  _data.rc_lost = false;
126  _data.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM;
127 
128  if (nullptr == _rcinput_pub) {
129  int instance;
130  _rcinput_pub = orb_advertise_multi(ORB_ID(input_rc), &_data, &instance, ORB_PRIO_DEFAULT);
131 
132  } else {
133  orb_publish(ORB_ID(input_rc), _rcinput_pub, &_data);
134  }
135 }
136 
137 static void rpi_rc_in::usage(const char *reason)
138 {
139  if (reason) {
140  PX4_ERR("%s", reason);
141  }
142 
143  PX4_INFO("rpi_rc_in {start|stop|status}");
144 }
145 
146 int rpi_rc_in_main(int argc, char **argv)
147 {
148  if (argc < 2) {
149  usage("missing command");
150  return 1;
151  }
152 
153  if (!strcmp(argv[1], "start")) {
154 
155  if (rc_input != nullptr && rc_input->is_running()) {
156  PX4_INFO("already running");
157  // this is not an error
158  return 0;
159  }
160 
161  rc_input = new RcInput();
162 
163  // Check if alloc worked.
164  if (nullptr == rc_input) {
165  PX4_ERR("Rc input module initialization faild");
166  return -1;
167  }
168 
169  int ret = rc_input->start();
170 
171  if (ret != 0) {
172  PX4_ERR("Rc input module failure");
173  }
174 
175  return 0;
176  }
177 
178  if (!strcmp(argv[1], "stop")) {
179 
180  if (rc_input == nullptr || !rc_input->is_running()) {
181  PX4_WARN("Not running");
182  // this is not an error
183  return 0;
184  }
185 
186  rc_input->stop();
187 
188  // Wait for task to die
189  int i = 0;
190 
191  do {
192  // wait for 100ms
193  usleep(100000);
194 
195  } while (rc_input->is_running() && ++i < 30);
196 
197  delete rc_input;
198  rc_input = nullptr;
199 
200  return 0;
201  }
202 
203  if (!strcmp(argv[1], "status")) {
204  if (rc_input != nullptr && rc_input->is_running()) {
205  PX4_INFO("running");
206 
207  } else {
208  PX4_INFO("Not running");
209  }
210 
211  return 0;
212  }
213 
214  usage("rpi_rc_in start|stop|status");
215  return 1;
216 
217 }
218 
int32_t rssi
Definition: input_rc.h:72
bool rc_failsafe
Definition: input_rc.h:77
#define RCINPUT_MEASURE_INTERVAL_US
Definition: linux_sbus.h:57
uint16_t rc_ppm_frame_length
Definition: input_rc.h:75
uint8_t input_source
Definition: input_rc.h:79
key_t _key
shared memory key (matches the ppmdecode program&#39;s key)
Definition: rpi_rc_in.h:95
uint64_t timestamp
Definition: input_rc.h:69
LidarLite * instance
Definition: ll40ls.cpp:65
uint16_t rc_lost_frame_count
Definition: input_rc.h:73
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
void Run() override
Definition: rpi_rc_in.cpp:95
static void usage(const char *reason)
Print the correct usage.
Definition: Commander.cpp:4238
uint16_t values[18]
Definition: input_rc.h:76
uint32_t channel_count
Definition: input_rc.h:71
orb_advert_t _rcinput_pub
Definition: rpi_rc_in.h:90
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
Definition: uORB.cpp:70
uint16_t rc_total_frame_count
Definition: input_rc.h:74
bool rc_lost
Definition: input_rc.h:78
int rpi_rc_in_main(int argc, char **argv)
Definition: rpi_rc_in.cpp:146
static RcInput * rc_input
Definition: rpi_rc_in.h:100
struct input_rc_s _data
Definition: rpi_rc_in.h:92
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority)
Definition: uORB.cpp:53
uint64_t timestamp_last_signal
Definition: input_rc.h:70
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).