41 #include <px4_platform_common/tasks.h> 42 #include <px4_platform_common/getopt.h> 43 #include <px4_platform_common/posix.h> 72 static char _device[64] =
"/sys/class/pwm/pwmchip0";
95 px4_pollfd_struct_t
_poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
120 static void task_main(
int argc,
char *argv[]);
130 uint8_t control_group,
131 uint8_t control_index,
135 input = controls[control_group].
control[control_index];
147 param_get(param_handle, (int32_t *)&airmode);
155 unsigned buflen =
sizeof(buf);
156 memset(buf,
'\0', buflen);
164 PX4_INFO(
"Loaded mixer from file %s", mixer_filename);
168 PX4_ERR(
"Unable to parse from mixer config file %s", mixer_filename);
172 PX4_ERR(
"Unable to load config file %s", mixer_filename);
175 if (_mixer_group->
count() <= 0) {
176 PX4_ERR(
"Mixer initialization failed");
186 memset(_controls, 0,
sizeof(_controls));
187 memset(_poll_fds, 0,
sizeof(_poll_fds));
190 _controls_topics[0] =
ORB_ID(actuator_controls_0);
191 _controls_topics[1] =
ORB_ID(actuator_controls_1);
192 _controls_topics[2] =
ORB_ID(actuator_controls_2);
193 _controls_topics[3] =
ORB_ID(actuator_controls_3);
196 for (uint8_t i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
197 if (_groups_required & (1 << i)) {
198 PX4_DEBUG(
"subscribe to actuator_controls_%d", i);
202 _controls_subs[i] = -1;
205 if (_controls_subs[i] >= 0) {
224 PX4_ERR(
"Mixer initialization failed.");
230 if (strcmp(_protocol,
"pca9685") == 0) {
231 PX4_INFO(
"Starting PWM output in PCA9685 mode");
234 }
else if (strcmp(_protocol,
"ocpoc_mmap") == 0) {
235 PX4_INFO(
"Starting PWM output in ocpoc_mmap mode");
240 }
else if (strcmp(_protocol,
"bbblue_rc") == 0) {
241 PX4_INFO(
"Starting PWM output in bbblue_rc mode");
246 PX4_INFO(
"Starting PWM output in Navio mode");
250 if (pwm_out->
init() != 0) {
251 PX4_ERR(
"PWM output init failed");
265 int rc_channels_sub = -1;
268 _armed.
armed =
false;
273 while (!_task_should_exit) {
286 int pret =
px4_poll(_poll_fds, _poll_fds_num, 10);
295 PX4_WARN(
"poll error %d, %d", pret, errno);
302 unsigned poll_id = 0;
304 for (uint8_t i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
305 if (_controls_subs[i] >= 0) {
306 if (_poll_fds[poll_id].revents & POLLIN) {
307 orb_copy(_controls_topics[i], _controls_subs[i], &_controls[i]);
315 if (rc_channels_sub == -1) {
321 int ret =
orb_copy(
ORB_ID(rc_channels), rc_channels_sub, &rc_channels);
325 int channel = rc_channels.
function[rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE];
327 if (ret == 0 && channel >= 0 && channel < (
int)(
sizeof(rc_channels.
channels) /
sizeof(rc_channels.
channels[0]))) {
338 if (_mixer_group !=
nullptr) {
340 _outputs.
noutputs = _mixer_group->
mix(_outputs.
output, actuator_outputs_s::NUM_ACTUATOR_OUTPUTS);
343 for (
size_t i = _outputs.
noutputs; i < _outputs.NUM_ACTUATOR_OUTPUTS; i++) {
347 const uint16_t reverse_mask = 0;
348 uint16_t disarmed_pwm[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS];
349 uint16_t min_pwm[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS];
350 uint16_t max_pwm[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS];
352 for (
unsigned int i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) {
358 uint16_t pwm[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS];
379 if (_controls[0].
control[3] > 0.5
f) {
386 for (uint32_t i = 0; i < _outputs.
noutputs; ++i) {
398 if (_outputs_pub !=
nullptr) {
406 for (
int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
407 const bool required = _groups_required & (1 << i);
410 if (required && (timestamp_sample > 0)) {
417 PX4_ERR(
"Could not mix output! Exiting...");
418 _task_should_exit =
true;
422 if (parameter_update_sub.updated()) {
425 parameter_update_sub.copy(&pupdate);
434 for (uint8_t i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
435 if (_controls_subs[i] >= 0) {
440 if (_armed_sub != -1) {
445 if (rc_channels_sub != -1) {
462 _task_should_exit =
false;
465 _task_handle = px4_task_spawn_cmd(
"pwm_out_main",
472 if (_task_handle < 0) {
473 PX4_ERR(
"task start failed");
481 _task_should_exit =
true;
483 while (_is_running) {
493 PX4_INFO(
"usage: pwm_out start [-d pwmdevice] [-m mixerfile] [-p protocol]");
494 PX4_INFO(
" -d pwmdevice : sysfs device for pwm generation (only for Navio)");
495 PX4_INFO(
" (default /sys/class/pwm/pwmchip0)");
496 PX4_INFO(
" -m mixerfile : path to mixerfile");
497 PX4_INFO(
" (default ROMFS/px4fmu_common/mixers/quad_x.main.mix)");
498 PX4_INFO(
" -p protocol : driver output protocol (navio|pca9685|ocpoc_mmap|bbblue_rc)");
499 PX4_INFO(
" (default is navio)");
500 PX4_INFO(
" -n num_outputs : maximum number of outputs the driver should use");
501 PX4_INFO(
" (default is 8)");
502 PX4_INFO(
" pwm_out stop");
503 PX4_INFO(
" pwm_out status");
515 const char *myoptarg =
nullptr;
517 char *verb =
nullptr;
526 while ((ch = px4_getopt(argc, argv,
"d:m:p:n:", &myoptind, &myoptarg)) != EOF) {
541 unsigned long max_num = strtoul(myoptarg,
nullptr, 10);
547 if (max_num > actuator_outputs_s::NUM_ACTUATOR_OUTPUTS) {
548 max_num = actuator_outputs_s::NUM_ACTUATOR_OUTPUTS;
565 if (!strcmp(verb,
"start")) {
567 PX4_WARN(
"pwm_out already running");
574 else if (!strcmp(verb,
"stop")) {
576 PX4_WARN(
"pwm_out is not running");
583 else if (!strcmp(verb,
"status")) {
int load_from_buf(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
Adds mixers to the group based on a text description in a buffer.
#define PARAM_INVALID
Handle returned when a parameter cannot be found.
static char _protocol[64]
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
static int _max_num_outputs
maximum number of outputs the driver should use
class NavioSysfsPWMOut PWM output class for Navio Sysfs
void output_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, const uint16_t reverse_mask, const uint16_t *disarmed_output, const uint16_t *min_output, const uint16_t *max_output, const float *output, uint16_t *effective_output, output_limit_t *limit)
class OcpocMmapPWMOut PWM output class for Aerotenna OcPoC via mmap
measure the time elapsed performing an event
orb_advert_t _outputs_pub
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
static px4_task_t _task_handle
class BBBlueRcPWMOut PWM output class for BeagleBone Blue with Robotics Cape Library ...
Main class that exports features for PCA9685 chip.
actuator_outputs_s _outputs
px4_pollfd_struct_t _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]
static void task_main_trampoline(int argc, char *argv[])
void perf_set_elapsed(perf_counter_t handle, int64_t elapsed)
Register a measurement.
orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data)
orb_id_t _controls_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]
actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]
unsigned count() const
Count the mixers in the group.
static void task_main(int argc, char *argv[])
High-resolution timer with callouts and timekeeping.
virtual int send_output_pwm(const uint16_t *pwm, int num_outputs)=0
Global flash based parameter store.
int orb_subscribe(const struct orb_metadata *meta)
int mixer_control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input)
bool in_esc_calibration_mode
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
enum output_limit_state state
void perf_free(perf_counter_t handle)
Free a counter.
int _controls_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]
int orb_unsubscribe(int handle)
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
uint32_t _groups_required
class PWMOutBase common abstract PWM output base class
int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
uint64_t timestamp_sample
static void update_params(Mixer::Airmode &airmode)
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
__EXPORT int linux_pwm_out_main(int argc, char *argv[])
volatile bool _task_should_exit
void groups_required(uint32_t &groups)
output_limit_t _pwm_limit
static char _mixer_filename[64]
void set_airmode(Mixer::Airmode airmode)
int orb_check(int handle, bool *updated)
void output_limit_init(output_limit_t *limit)
MixerGroup * _mixer_group
unsigned mix(float *outputs, unsigned space)
Group of mixers, built up from single mixers and processed in order when mixing.
uint32_t _groups_subscribed
perf_counter_t _perf_control_latency
int initialize_mixer(const char *mixer_filename)
Library for output limiting (PWM for example)
11 bits per channel * 16 channels = 22 bytes.
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
uint32_t param_t
Parameter handle.
Performance measuring tools.