PX4 Firmware
PX4 Autopilot Software http://px4.io
PWMSim.cpp
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 #include "PWMSim.hpp"
35 
36 #include <mathlib/mathlib.h>
37 #include <px4_platform_common/getopt.h>
38 
39 #include <uORB/Subscription.hpp>
41 
42 PWMSim::PWMSim(bool hil_mode_enabled) :
44  OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default)
45 {
50 
51  _mixing_output.setIgnoreLockdown(hil_mode_enabled);
52 
53  CDev::init();
54 }
55 
56 void
58 {
59  if (should_exit()) {
60  ScheduleClear();
62 
63  exit_and_cleanup();
64  return;
65  }
66 
68 
69  // check for parameter updates
71  parameter_update_s pupdate;
72  _parameter_update_sub.copy(&pupdate);
73  updateParams();
74  }
75 
76  // check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread)
78 }
79 
80 bool
81 PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
82  unsigned num_control_groups_updated)
83 {
84  // Nothing to do, as we are only interested in the actuator_outputs topic publication.
85  // That should only be published once we receive actuator_controls (important for lock-step to work correctly)
86  return num_control_groups_updated > 0;
87 }
88 
89 int
90 PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg)
91 {
92  int ret = OK;
93 
94  lock();
95 
96  switch (cmd) {
97  case PWM_SERVO_ARM:
98  break;
99 
100  case PWM_SERVO_DISARM:
101  break;
102 
103  case PWM_SERVO_SET_MIN_PWM: {
104  struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
105 
106  for (unsigned i = 0; i < pwm->channel_count; i++) {
108  _mixing_output.minValue(i) = pwm->values[i];
109  }
110  }
111 
112  break;
113  }
114 
115  case PWM_SERVO_SET_MAX_PWM: {
116  struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
117 
118  for (unsigned i = 0; i < pwm->channel_count; i++) {
120  _mixing_output.maxValue(i) = pwm->values[i];
121  }
122  }
123 
124  break;
125  }
126 
128  // PWMSim does not limit the update rate
129  break;
130 
132  break;
133 
135  *(uint32_t *)arg = 9999;
136  break;
137 
139  *(uint32_t *)arg = 9999;
140  break;
141 
143  *(uint32_t *)arg = 0;
144  break;
145 
147  struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
148 
149  for (unsigned i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) {
150  pwm->values[i] = _mixing_output.failsafeValue(i);
151  }
152 
154  break;
155  }
156 
158  struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
159 
160  for (unsigned i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) {
161  pwm->values[i] = _mixing_output.disarmedValue(i);
162  }
163 
165  break;
166  }
167 
168  case PWM_SERVO_GET_MIN_PWM: {
169  struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
170 
171  for (unsigned i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) {
172  pwm->values[i] = _mixing_output.minValue(i);
173  }
174 
176  break;
177  }
178 
179  case PWM_SERVO_GET_TRIM_PWM: {
180  struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
181 
182  for (unsigned i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) {
183  pwm->values[i] = (_mixing_output.maxValue(i) + _mixing_output.minValue(i)) / 2;
184  }
185 
187  break;
188  }
189 
190  case PWM_SERVO_GET_MAX_PWM: {
191  struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
192 
193  for (unsigned i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) {
194  pwm->values[i] = _mixing_output.maxValue(i);
195  }
196 
198  break;
199  }
200 
202  // no restrictions on output grouping
203  unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0);
204 
205  *(uint32_t *)arg = (1 << channel);
206  break;
207  }
208 
209  case PWM_SERVO_GET_COUNT:
210  *(unsigned *)arg = OutputModuleInterface::MAX_ACTUATORS;
211  break;
212 
213  case MIXERIOCRESET:
215  break;
216 
217  case MIXERIOCLOADBUF: {
218  const char *buf = (const char *)arg;
219  unsigned buflen = strlen(buf);
220  ret = _mixing_output.loadMixerThreadSafe(buf, buflen);
221  break;
222  }
223 
224 
225  default:
226  ret = -ENOTTY;
227  break;
228  }
229 
230  unlock();
231 
232  return ret;
233 }
234 
235 int
236 PWMSim::task_spawn(int argc, char *argv[])
237 {
238  bool hil_mode = false;
239 
240  int myoptind = 1;
241  int ch;
242  const char *myoptarg = nullptr;
243 
244  while ((ch = px4_getopt(argc, argv, "m:", &myoptind, &myoptarg)) != EOF) {
245  switch (ch) {
246  case 'm':
247  hil_mode = strcmp(myoptarg, "hil") == 0;
248  break;
249 
250  default:
251  return print_usage("unrecognized flag");
252  }
253  }
254 
255  PWMSim *instance = new PWMSim(hil_mode);
256 
257  if (!instance) {
258  PX4_ERR("alloc failed");
259  return -1;
260  }
261 
262  _object.store(instance);
263  _task_id = task_id_is_work_queue;
264  instance->ScheduleNow();
265  return 0;
266 }
267 
268 int PWMSim::custom_command(int argc, char *argv[])
269 {
270  return print_usage("unknown command");
271 }
272 
274 {
276  return 0;
277 }
278 
279 int PWMSim::print_usage(const char *reason)
280 {
281  if (reason) {
282  PX4_WARN("%s\n", reason);
283  }
284 
285  PRINT_MODULE_DESCRIPTION(
286  R"DESCR_STR(
287 ### Description
288 Driver for simulated PWM outputs.
289 
290 Its only function is to take `actuator_control` uORB messages,
291 mix them with any loaded mixer and output the result to the
292 `actuator_output` uORB topic.
293 
294 It is used in SITL and HITL.
295 
296 )DESCR_STR");
297 
298  PRINT_MODULE_USAGE_NAME("pwm_out_sim", "driver");
299  PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the module");
300  PRINT_MODULE_USAGE_PARAM_STRING('m', "sim", "hil|sim", "Mode", true);
301  PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
302 
303  return 0;
304 }
305 
306 extern "C" __EXPORT int pwm_out_sim_main(int argc, char *argv[])
307 {
308  return PWMSim::main(argc, argv);
309 }
#define PWM_SERVO_GET_SELECT_UPDATE_RATE
check the selected update rates
#define MIXERIOCLOADBUF
Add mixer(s) from the buffer in (const char *)arg.
Definition: drv_mixer.h:79
static constexpr uint16_t PWM_SIM_PWM_MIN_MAGIC
Definition: PWMSim.hpp:74
void printStatus() const
#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
static int print_usage(const char *reason=nullptr)
Definition: PWMSim.cpp:279
int main(int argc, char **argv)
Definition: main.cpp:3
void lock()
Take the driver lock.
Definition: CDev.hpp:264
PWMSim(bool hil_mode_enabled)
Definition: PWMSim.cpp:42
Definition: I2C.hpp:51
int ioctl(device::file_t *filp, int cmd, unsigned long arg) override
Perform an ioctl operation on the device.
Definition: PWMSim.cpp:90
#define PWM_SERVO_GET_UPDATE_RATE
get alternate servo update rate
#define PWM_SERVO_GET_DISARMED_PWM
get the PWM value when disarmed
bool update()
Call this regularly from Run().
__EXPORT int pwm_out_sim_main(int argc, char *argv[])
Definition: PWMSim.cpp:306
static constexpr uint16_t PWM_SIM_PWM_MAX_MAGIC
Definition: PWMSim.hpp:75
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.
static constexpr int MAX_ACTUATORS
void setIgnoreLockdown(bool ignore_lockdown)
static int custom_command(int argc, char *argv[])
Definition: PWMSim.cpp:268
static constexpr uint16_t PWM_SIM_DISARMED_MAGIC
Definition: PWMSim.hpp:72
#define PWM_SERVO_GET_DEFAULT_UPDATE_RATE
get default servo update rate
#define PWM_OUTPUT0_DEVICE_PATH
uint16_t values[PWM_OUTPUT_MAX_CHANNELS]
uint16_t & maxValue(int index)
void init()
Activates/configures the hardware registers.
MixingOutput _mixing_output
Definition: PWMSim.hpp:77
static int task_spawn(int argc, char *argv[])
Definition: PWMSim.cpp:236
int print_status() override
Definition: PWMSim.cpp:273
void setAllFailsafeValues(uint16_t value)
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 PWM_OUTPUT_MAX_CHANNELS
bool updated()
Check if there is a new update.
Base class for an output module.
#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...
int loadMixerThreadSafe(const char *buf, unsigned len)
Load (append) a new mixer from a buffer, called from another thread.
void Run() override
Definition: PWMSim.cpp:57
#define PWM_SERVO_SET_MIN_PWM
set the minimum PWM value the output will send
uORB::Subscription _parameter_update_sub
Definition: PWMSim.hpp:78
#define MIXERIOCRESET
reset (clear) the mixer configuration
Definition: drv_mixer.h:71
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: PWMSim.cpp:81
void setAllMaxValues(uint16_t value)
#define PWM_SERVO_SET_MAX_PWM
set the maximum PWM value the output will send
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
void setAllDisarmedValues(uint16_t value)
Definition: bst.cpp:62
uint16_t & minValue(int index)
#define OK
Definition: uavcan_main.cpp:71
static constexpr uint16_t PWM_SIM_FAILSAFE_MAGIC
Definition: PWMSim.hpp:73
void unregister()
unregister uORB subscription callbacks
void unlock()
Release the driver lock.
Definition: CDev.hpp:269
#define PWM_SERVO_GET_FAILSAFE_PWM
get the PWM value for failsafe
bool copy(void *dst)
Copy the struct.
#define PWM_SERVO_DISARM
disarm all servo outputs (stop generating pulses)
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
void setAllMinValues(uint16_t value)