36 #include <px4_platform_common/tasks.h> 37 #include <px4_platform_common/getopt.h> 38 #include <px4_platform_common/posix.h> 58 #include <dev_fs_lib_pwm.h> 78 static struct ::dspal_pwm *
_pwm;
103 px4_pollfd_struct_t
_poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
140 static void task_main(
int argc,
char *argv[]);
155 uint8_t control_group,
156 uint8_t control_index,
161 input = controls[control_group].
control[control_index];
179 size_t buflen =
sizeof(buf);
180 PX4_INFO(
"Trying to initialize mixer from config file %s", mixer_filename);
181 int fd_load = ::open(mixer_filename, O_RDONLY);
183 int nRead =
::read(fd_load, buf, buflen);
189 if (_mixer !=
nullptr) {
190 PX4_INFO(
"Successfully initialized mixer from config file");
194 PX4_ERR(
"Unable to parse from mixer config file");
199 PX4_WARN(
"Unable to read from mixer config file");
206 memset(_controls, 0,
sizeof(_controls));
207 memset(_poll_fds, 0,
sizeof(_poll_fds));
212 _controls_topics[0] =
ORB_ID(actuator_controls_0);
213 _controls_topics[1] =
ORB_ID(actuator_controls_1);
214 _controls_topics[2] =
ORB_ID(actuator_controls_2);
215 _controls_topics[3] =
ORB_ID(actuator_controls_3);
221 for (uint8_t i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
223 if (_groups_required & (1 << i)) {
225 PX4_DEBUG(
"subscribe to actuator_controls_%d", i);
231 _controls_subs[i] = -1;
235 if (_controls_subs[i] >= 0) {
256 _fd = open(_device, 0);
259 PX4_ERR(
"failed to open PWM device!");
266 for (
int i = 0; i <
NUM_PWM; i++) {
267 _pwm_gpio[i].gpio_id = PIN_GPIO + i;
273 _signal_definition.num_gpios =
NUM_PWM;
275 _signal_definition.pwm_signal =
_pwm_gpio;
281 if (::ioctl(_fd, PWM_IOCTL_SIGNAL_DEFINITION, &_signal_definition) != 0) {
282 PX4_ERR(
"failed to send signal to DSP");
289 if (::ioctl(_fd, PWM_IOCTL_GET_UPDATE_BUFFER, &_update_buffer) != 0) {
290 PX4_ERR(
"failed to receive update buffer ");
294 _pwm = _update_buffer->pwm_signal;
312 for (
unsigned i = 0; i <
NUM_PWM; ++i) {
313 _pwm[i].pulse_width_in_usecs = pwm[i];
320 PX4_ERR(
"Failed to initialize PWM.");
325 PX4_ERR(
"Mixer initialization failed.");
339 _armed.
armed =
false;
350 while (!_task_should_exit) {
357 int pret =
px4_poll(_poll_fds, _poll_fds_num, 10);
360 bool timeout =
false;
368 PX4_WARN(
"poll error %d, %d", pret, errno);
382 unsigned poll_id = 0;
384 for (uint8_t i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
385 if (_controls_subs[i] >= 0) {
386 if (_poll_fds[poll_id].revents & POLLIN) {
387 orb_copy(_controls_topics[i], _controls_subs[i], &_controls[i]);
394 if (_mixer ==
nullptr) {
395 PX4_ERR(
"Could not mix output! Exiting...");
396 _task_should_exit =
true;
404 const uint16_t reverse_mask = 0;
405 uint16_t disarmed_pwm[4];
410 for (
unsigned int i = 0; i < 4; i++) {
419 false, _outputs.
noutputs, reverse_mask, disarmed_pwm,
420 min_pwm, max_pwm, _outputs.
output, pwm, &_pwm_limit);
432 if (_outputs_pub !=
nullptr) {
440 for (
int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
441 const bool required = _groups_required & (1 << i);
444 if (required && (timestamp_sample > 0)) {
451 if (parameter_update_sub.updated()) {
454 parameter_update_sub.copy(&pupdate);
463 for (uint8_t i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
464 if (_controls_subs[i] >= 0) {
483 _task_should_exit =
false;
486 _task_handle = px4_task_spawn_cmd(
"snapdragon_pwm_out_main",
493 if (_task_handle < 0) {
494 PX4_ERR(
"task start failed");
502 _task_should_exit =
true;
504 while (_is_running) {
515 PX4_INFO(
"usage: snapdragon_pwm_out start [-d pwmdevice] [-m mixerfile]");
516 PX4_INFO(
" -d pwmdevice : device for pwm generation");
517 PX4_INFO(
" (default /dev/pwm-1)");
518 PX4_INFO(
" -m mixerfile : path to mixerfile");
519 PX4_INFO(
" (default /dev/fs/quad_x.main.mix");
520 PX4_INFO(
" pwm_out stop");
521 PX4_INFO(
" pwm_out status");
536 const char *myoptarg =
nullptr;
538 char *verb =
nullptr;
547 while ((ch = px4_getopt(argc, argv,
"d:m", &myoptind, &myoptarg)) != EOF) {
567 if (!strcmp(verb,
"start")) {
569 PX4_WARN(
"pwm_out already running");
576 else if (!strcmp(verb,
"stop")) {
578 PX4_WARN(
"pwm_out is not running");
585 else if (!strcmp(verb,
"status")) {
int initialize_mixer(const char *mixer_filename)
#define PARAM_INVALID
Handle returned when a parameter cannot be found.
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
static const int FREQUENCY_PWM_HZ
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)
measure the time elapsed performing an event
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
static MultirotorMixer * from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
Factory method.
perf_counter_t _perf_control_latency
uint32_t _groups_required
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)
static char _mixer_filename[32]
Namespace encapsulating all device framework classes, functions and data.
High-resolution timer with callouts and timekeeping.
static struct ::dspal_pwm _pwm_gpio[NUM_PWM]
Global flash based parameter store.
void set_airmode(Airmode airmode) override
Set airmode.
__EXPORT int snapdragon_pwm_out_main(int argc, char *argv[])
static void read(bootloader_app_shared_t *pshared)
int orb_subscribe(const struct orb_metadata *meta)
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
static px4_task_t _task_handle
static void task_main_trampoline(int argc, char *argv[])
void perf_free(perf_counter_t handle)
Free a counter.
orb_advert_t _outputs_pub
int orb_unsubscribe(int handle)
orb_id_t _controls_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]
static void task_main(int argc, char *argv[])
static struct ::dspal_pwm * _pwm
uint64_t timestamp_sample
px4_pollfd_struct_t _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]
int _controls_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]
__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)
static void pwm_deinitialize()
int mixer_control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input)
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
static const int PIN_GPIO
unsigned mix(float *outputs, unsigned space) override
Perform the mixing function.
volatile bool _task_should_exit
output_limit_t _pwm_limit
int orb_check(int handle, bool *updated)
static struct ::dspal_pwm_ioctl_signal_definition _signal_definition
void output_limit_init(output_limit_t *limit)
static struct ::dspal_pwm_ioctl_update_buffer * _update_buffer
void groups_required(uint32_t &groups) override
Analyses the mix configuration and updates a bitmask of groups that are required. ...
Multi-rotor mixer for pre-defined vehicle geometries.
actuator_outputs_s _outputs
static void send_outputs_pwm(const uint16_t *pwm)
Library for output limiting (PWM for example)
static void update_params(Mixer::Airmode &airmode)
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
uint32_t param_t
Parameter handle.
actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]
Performance measuring tools.
static int pwm_initialize(const char *device)