dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

pwm systemcmd can now set the failsafe values, fmu uses failsafe values as well now, fix to only send the appropriate number of pwm values to IO at once

This commit is contained in:
Julian Oes 2013-10-19 10:43:41 +02:00
parent 5d36971689
commit 1d3f25ee6c
5 changed files with 233 additions and 174 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
src/drivers/drv_pwm_output.h
View File

@ -154,23 +154,29 @@ ORB_DECLARE(output_pwm);
/** power up DSM receiver */ /** power up DSM receiver */
#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 11) #define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 11)
/** set the PWM value for failsafe */
#define PWM_SERVO_SET_FAILSAFE_PWM _IOC(_PWM_SERVO_BASE, 12)
/** get the PWM value for failsafe */
#define PWM_SERVO_GET_FAILSAFE_PWM _IOC(_PWM_SERVO_BASE, 13)
/** set the PWM value when disarmed - should be no PWM (zero) by default */ /** set the PWM value when disarmed - should be no PWM (zero) by default */
#define PWM_SERVO_SET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 12) #define PWM_SERVO_SET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 14)
/** get the PWM value when disarmed */ /** get the PWM value when disarmed */
#define PWM_SERVO_GET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 13) #define PWM_SERVO_GET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 15)
/** set the minimum PWM value the output will send */ /** set the minimum PWM value the output will send */
#define PWM_SERVO_SET_MIN_PWM _IOC(_PWM_SERVO_BASE, 14) #define PWM_SERVO_SET_MIN_PWM _IOC(_PWM_SERVO_BASE, 16)
/** get the minimum PWM value the output will send */ /** get the minimum PWM value the output will send */
#define PWM_SERVO_GET_MIN_PWM _IOC(_PWM_SERVO_BASE, 15) #define PWM_SERVO_GET_MIN_PWM _IOC(_PWM_SERVO_BASE, 17)
/** set the maximum PWM value the output will send */ /** set the maximum PWM value the output will send */
#define PWM_SERVO_SET_MAX_PWM _IOC(_PWM_SERVO_BASE, 16) #define PWM_SERVO_SET_MAX_PWM _IOC(_PWM_SERVO_BASE, 18)
/** get the maximum PWM value the output will send */ /** get the maximum PWM value the output will send */
#define PWM_SERVO_GET_MAX_PWM _IOC(_PWM_SERVO_BASE, 17) #define PWM_SERVO_GET_MAX_PWM _IOC(_PWM_SERVO_BASE, 19)
/** set a single servo to a specific value */ /** set a single servo to a specific value */
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)

202
src/drivers/px4fmu/fmu.cpp
View File

@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2012 PX4 Development Team. All rights reserved. * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -78,6 +78,12 @@
# include <systemlib/ppm_decode.h> # include <systemlib/ppm_decode.h>
#endif #endif
/*
* This is the analog to FMU_INPUT_DROP_LIMIT_US on the IO side
*/
#define CONTROL_INPUT_DROP_LIMIT_MS 20
class PX4FMU : public device::CDev class PX4FMU : public device::CDev
{ {
public: public:
@ -130,9 +136,11 @@ private:
actuator_controls_s _controls; actuator_controls_s _controls;
pwm_limit_t _pwm_limit; pwm_limit_t _pwm_limit;
uint16_t _failsafe_pwm[_max_actuators];
uint16_t _disarmed_pwm[_max_actuators]; uint16_t _disarmed_pwm[_max_actuators];
uint16_t _min_pwm[_max_actuators]; uint16_t _min_pwm[_max_actuators];
uint16_t _max_pwm[_max_actuators]; uint16_t _max_pwm[_max_actuators];
unsigned _num_failsafe_set;
unsigned _num_disarmed_set; unsigned _num_disarmed_set;
static void task_main_trampoline(int argc, char *argv[]); static void task_main_trampoline(int argc, char *argv[]);
@ -218,7 +226,9 @@ PX4FMU::PX4FMU() :
_armed(false), _armed(false),
_pwm_on(false), _pwm_on(false),
_mixers(nullptr), _mixers(nullptr),
_failsafe_pwm({0}),
_disarmed_pwm({0}), _disarmed_pwm({0}),
_num_failsafe_set(0),
_num_disarmed_set(0) _num_disarmed_set(0)
{ {
for (unsigned i = 0; i < _max_actuators; i++) { for (unsigned i = 0; i < _max_actuators; i++) {
@ -515,98 +525,103 @@ PX4FMU::task_main()
/* sleep waiting for data, stopping to check for PPM /* sleep waiting for data, stopping to check for PPM
* input at 100Hz */ * input at 100Hz */
int ret = ::poll(&fds[0], 2, 10); int ret = ::poll(&fds[0], 2, CONTROL_INPUT_DROP_LIMIT_MS);
/* this would be bad... */ /* this would be bad... */
if (ret < 0) { if (ret < 0) {
log("poll error %d", errno); log("poll error %d", errno);
usleep(1000000); usleep(1000000);
continue; continue;
} } else if (ret == 0) {
/* timeout: no control data, switch to failsafe values */
// warnx("no PWM: failsafe");
/* do we have a control update? */ } else {
if (fds[0].revents & POLLIN) {
/* get controls - must always do this to avoid spinning */ /* do we have a control update? */
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); if (fds[0].revents & POLLIN) {
/* can we mix? */ /* get controls - must always do this to avoid spinning */
if (_mixers != nullptr) { orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
unsigned num_outputs; /* can we mix? */
if (_mixers != nullptr) {
switch (_mode) { unsigned num_outputs;
case MODE_2PWM:
num_outputs = 2;
break;
case MODE_4PWM:
num_outputs = 4;
break;
case MODE_6PWM:
num_outputs = 6;
break;
default:
num_outputs = 0;
break;
}
/* do mixing */ switch (_mode) {
outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); case MODE_2PWM:
outputs.timestamp = hrt_absolute_time(); num_outputs = 2;
break;
/* iterate actuators */ case MODE_4PWM:
for (unsigned i = 0; i < num_outputs; i++) { num_outputs = 4;
/* last resort: catch NaN, INF and out-of-band errors */ break;
if (i >= outputs.noutputs || case MODE_6PWM:
!isfinite(outputs.output[i]) || num_outputs = 6;
outputs.output[i] < -1.0f || break;
outputs.output[i] > 1.0f) { default:
/* num_outputs = 0;
* Value is NaN, INF or out of band - set to the minimum value. break;
* This will be clearly visible on the servo status and will limit the risk of accidentally
* spinning motors. It would be deadly in flight.
*/
outputs.output[i] = -1.0f;
} }
/* do mixing */
outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
outputs.timestamp = hrt_absolute_time();
/* iterate actuators */
for (unsigned i = 0; i < num_outputs; i++) {
/* last resort: catch NaN, INF and out-of-band errors */
if (i >= outputs.noutputs ||
!isfinite(outputs.output[i]) ||
outputs.output[i] < -1.0f ||
outputs.output[i] > 1.0f) {
/*
* Value is NaN, INF or out of band - set to the minimum value.
* This will be clearly visible on the servo status and will limit the risk of accidentally
* spinning motors. It would be deadly in flight.
*/
outputs.output[i] = -1.0f;
}
}
uint16_t pwm_limited[num_outputs];
pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);
/* output actual limited values */
for (unsigned i = 0; i < num_outputs; i++) {
controls_effective.control_effective[i] = (float)pwm_limited[i];
}
orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
/* output to the servos */
for (unsigned i = 0; i < num_outputs; i++) {
up_pwm_servo_set(i, pwm_limited[i]);
}
/* and publish for anyone that cares to see */
orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs);
} }
uint16_t pwm_limited[num_outputs];
pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);
/* output actual limited values */
for (unsigned i = 0; i < num_outputs; i++) {
controls_effective.control_effective[i] = (float)pwm_limited[i];
}
orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
/* output to the servos */
for (unsigned i = 0; i < num_outputs; i++) {
up_pwm_servo_set(i, pwm_limited[i]);
}
/* and publish for anyone that cares to see */
orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs);
} }
}
/* how about an arming update? */ /* how about an arming update? */
if (fds[1].revents & POLLIN) { if (fds[1].revents & POLLIN) {
actuator_armed_s aa; actuator_armed_s aa;
/* get new value */ /* get new value */
orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa); orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa);
/* update the armed status and check that we're not locked down */ /* update the armed status and check that we're not locked down */
bool set_armed = aa.armed && !aa.lockdown; bool set_armed = aa.armed && !aa.lockdown;
if (_armed != set_armed) if (_armed != set_armed)
_armed = set_armed; _armed = set_armed;
/* update PWM status if armed or if disarmed PWM values are set */ /* update PWM status if armed or if disarmed PWM values are set */
bool pwm_on = (aa.armed || _num_disarmed_set > 0); bool pwm_on = (aa.armed || _num_disarmed_set > 0);
if (_pwm_on != pwm_on) { if (_pwm_on != pwm_on) {
_pwm_on = pwm_on; _pwm_on = pwm_on;
up_pwm_servo_arm(pwm_on); up_pwm_servo_arm(pwm_on);
}
} }
} }
@ -737,6 +752,45 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
*(uint32_t *)arg = _pwm_alt_rate_channels; *(uint32_t *)arg = _pwm_alt_rate_channels;
break; break;
case PWM_SERVO_SET_FAILSAFE_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
/* discard if too many values are sent */
if (pwm->channel_count > _max_actuators) {
ret = -EINVAL;
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] > PWM_MAX) {
_failsafe_pwm[i] = PWM_MAX;
} else if (pwm->values[i] < PWM_MIN) {
_failsafe_pwm[i] = PWM_MIN;
} else {
_failsafe_pwm[i] = pwm->values[i];
}
}
/*
* update the counter
* this is needed to decide if disarmed PWM output should be turned on or not
*/
_num_failsafe_set = 0;
for (unsigned i = 0; i < _max_actuators; i++) {
if (_failsafe_pwm[i] > 0)
_num_failsafe_set++;
}
break;
}
case PWM_SERVO_GET_FAILSAFE_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _failsafe_pwm[i];
}
pwm->channel_count = _max_actuators;
break;
}
case PWM_SERVO_SET_DISARMED_PWM: { case PWM_SERVO_SET_DISARMED_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values*)arg; struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
/* discard if too many values are sent */ /* discard if too many values are sent */

128
src/drivers/px4io/px4io.cpp
View File

@ -943,53 +943,6 @@ PX4IO::io_set_control_state()
return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls); return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls);
} }
int
PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len)
{
if (len > _max_actuators)
/* fail with error */
return E2BIG;
/* copy values to registers in IO */
return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, vals, len);
}
int
PX4IO::set_min_values(const uint16_t *vals, unsigned len)
{
if (len > _max_actuators)
/* fail with error */
return E2BIG;
/* copy values to registers in IO */
return io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, vals, len);
}
int
PX4IO::set_max_values(const uint16_t *vals, unsigned len)
{
if (len > _max_actuators)
/* fail with error */
return E2BIG;
/* copy values to registers in IO */
return io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, vals, len);
}
int
PX4IO::set_idle_values(const uint16_t *vals, unsigned len)
{
if (len > _max_actuators)
/* fail with error */
return E2BIG;
/* copy values to registers in IO */
return io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, vals, len);
}
int int
PX4IO::io_set_arming_state() PX4IO::io_set_arming_state()
@ -1803,7 +1756,7 @@ PX4IO::print_status()
printf("failsafe"); printf("failsafe");
for (unsigned i = 0; i < _max_actuators; i++) for (unsigned i = 0; i < _max_actuators; i++)
printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i));
printf("\nidle values"); printf("\ndisarmed values");
for (unsigned i = 0; i < _max_actuators; i++) for (unsigned i = 0; i < _max_actuators; i++)
printf(" %u", io_reg_get(PX4IO_PAGE_DISARMED_PWM, i)); printf(" %u", io_reg_get(PX4IO_PAGE_DISARMED_PWM, i));
printf("\n"); printf("\n");
@ -1874,15 +1827,39 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
*(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES); *(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES);
break; break;
case PWM_SERVO_SET_FAILSAFE_PWM: {
struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
if (pwm->channel_count > _max_actuators)
/* fail with error */
return E2BIG;
/* copy values to registers in IO */
ret = io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm->values, pwm->channel_count);
break;
}
case PWM_SERVO_GET_FAILSAFE_PWM:
ret = io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, 0, (uint16_t*)arg, _max_actuators);
if (ret != OK) {
ret = -EIO;
}
break;
case PWM_SERVO_SET_DISARMED_PWM: { case PWM_SERVO_SET_DISARMED_PWM: {
struct pwm_output_values* pwm = (struct pwm_output_values*)arg; struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
set_idle_values(pwm->values, pwm->channel_count); if (pwm->channel_count > _max_actuators)
/* fail with error */
return E2BIG;
/* copy values to registers in IO */
ret = io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, pwm->values, pwm->channel_count);
break; break;
} }
case PWM_SERVO_GET_DISARMED_PWM: case PWM_SERVO_GET_DISARMED_PWM:
ret = io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, (uint16_t*)arg, sizeof(struct pwm_output_values)/sizeof(uint16_t)); ret = io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, (uint16_t*)arg, _max_actuators);
if (ret != OK) { if (ret != OK) {
ret = -EIO; ret = -EIO;
} }
@ -1890,13 +1867,18 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
case PWM_SERVO_SET_MIN_PWM: { case PWM_SERVO_SET_MIN_PWM: {
struct pwm_output_values* pwm = (struct pwm_output_values*)arg; struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
set_min_values(pwm->values, pwm->channel_count); if (pwm->channel_count > _max_actuators)
/* fail with error */
return E2BIG;
/* copy values to registers in IO */
ret = io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm->values, pwm->channel_count);
break; break;
} }
case PWM_SERVO_GET_MIN_PWM: case PWM_SERVO_GET_MIN_PWM:
ret = io_reg_get(PX4IO_PAGE_CONTROL_MIN_PWM, 0, (uint16_t*)arg, sizeof(struct pwm_output_values)/sizeof(uint16_t)); ret = io_reg_get(PX4IO_PAGE_CONTROL_MIN_PWM, 0, (uint16_t*)arg, _max_actuators);
if (ret != OK) { if (ret != OK) {
ret = -EIO; ret = -EIO;
} }
@ -1904,13 +1886,18 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
case PWM_SERVO_SET_MAX_PWM: { case PWM_SERVO_SET_MAX_PWM: {
struct pwm_output_values* pwm = (struct pwm_output_values*)arg; struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
set_max_values(pwm->values, pwm->channel_count); if (pwm->channel_count > _max_actuators)
/* fail with error */
return E2BIG;
/* copy values to registers in IO */
ret = io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm->values, pwm->channel_count);
break; break;
} }
case PWM_SERVO_GET_MAX_PWM: case PWM_SERVO_GET_MAX_PWM:
ret = io_reg_get(PX4IO_PAGE_CONTROL_MAX_PWM, 0, (uint16_t*)arg, sizeof(struct pwm_output_values)/sizeof(uint16_t)); ret = io_reg_get(PX4IO_PAGE_CONTROL_MAX_PWM, 0, (uint16_t*)arg, _max_actuators);
if (ret != OK) { if (ret != OK) {
ret = -EIO; ret = -EIO;
} }
@ -2518,37 +2505,6 @@ px4io_main(int argc, char *argv[])
exit(0); exit(0);
} }
if (!strcmp(argv[1], "failsafe")) {
if (argc < 3) {
errx(1, "failsafe command needs at least one channel value (ppm)");
}
/* set values for first 8 channels, fill unassigned channels with 1500. */
uint16_t failsafe[8];
for (unsigned i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++) {
/* set channel to commandline argument or to 900 for non-provided channels */
if ((unsigned)argc > i + 2) {
failsafe[i] = atoi(argv[i+2]);
if (failsafe[i] < PWM_MIN || failsafe[i] > PWM_MAX) {
errx(1, "value out of range of %d < value < %d. Aborting.", PWM_MIN, PWM_MAX);
}
} else {
/* a zero value will result in stopping to output any pulse */
failsafe[i] = 0;
}
}
int ret = g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0]));
if (ret != OK)
errx(ret, "failed setting failsafe values");
exit(0);
}
if (!strcmp(argv[1], "recovery")) { if (!strcmp(argv[1], "recovery")) {
@ -2616,5 +2572,5 @@ px4io_main(int argc, char *argv[])
bind(argc, argv); bind(argc, argv);
out: out:
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'failsafe', 'bind' or 'update'"); errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind' or 'update'");
} }

13
src/modules/px4iofirmware/registers.c
View File

@ -199,7 +199,7 @@ uint16_t r_page_rc_input_config[PX4IO_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRI
* *
* Disable pulses as default. * Disable pulses as default.
*/ */
uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0 }; uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 };
/** /**
* PAGE 106 * PAGE 106
@ -276,8 +276,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
/* copy channel data */ /* copy channel data */
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
/* XXX range-check value? */ if (*values == 0) {
r_page_servo_failsafe[offset] = *values; /* ignore 0 */
} else if (*values < PWM_MIN) {
r_page_servo_failsafe[offset] = PWM_MIN;
} else if (*values > PWM_MAX) {
r_page_servo_failsafe[offset] = PWM_MAX;
} else {
r_page_servo_failsafe[offset] = *values;
}
/* flag the failsafe values as custom */ /* flag the failsafe values as custom */
r_setup_arming |= PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM; r_setup_arming |= PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM;

46
src/systemcmds/pwm/pwm.c
View File

@ -72,20 +72,21 @@ usage(const char *reason)
warnx("%s", reason); warnx("%s", reason);
errx(1, errx(1,
"usage:\n" "usage:\n"
"pwm arm|disarm|rate|min|max|disarmed|test|info ...\n" "pwm arm|disarm|rate|failsafe|disarmed|min|max|test|info ...\n"
"\n" "\n"
" arm Arm output\n" " arm Arm output\n"
" disarm Disarm output\n" " disarm Disarm output\n"
"\n" "\n"
" rate Configure PWM rates\n" " rate ... Configure PWM rates\n"
" [-g <channel group>] Channel group that should update at the alternate rate\n" " [-g <channel group>] Channel group that should update at the alternate rate\n"
" [-m <chanmask> ] Directly supply channel mask\n" " [-m <chanmask> ] Directly supply channel mask\n"
" [-a] Configure all outputs\n" " [-a] Configure all outputs\n"
" -r <alt_rate> PWM rate (50 to 400 Hz)\n" " -r <alt_rate> PWM rate (50 to 400 Hz)\n"
"\n" "\n"
" failsafe ... Configure failsafe PWM values\n"
" disarmed ... Configure disarmed PWM values\n"
" min ... Configure minimum PWM values\n" " min ... Configure minimum PWM values\n"
" max ... Configure maximum PWM values\n" " max ... Configure maximum PWM values\n"
" disarmed ... Configure disarmed PWM values\n"
" [-c <channels>] Supply channels (e.g. 1234)\n" " [-c <channels>] Supply channels (e.g. 1234)\n"
" [-m <chanmask> ] Directly supply channel mask (e.g. 0xF)\n" " [-m <chanmask> ] Directly supply channel mask (e.g. 0xF)\n"
" [-a] Configure all outputs\n" " [-a] Configure all outputs\n"
@ -357,6 +358,35 @@ pwm_main(int argc, char *argv[])
} }
exit(0); exit(0);
} else if (!strcmp(argv[1], "failsafe")) {
if (set_mask == 0) {
usage("no channels set");
}
if (pwm_value == 0)
usage("no PWM value provided");
struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
for (unsigned i = 0; i < servo_count; i++) {
if (set_mask & 1<<i) {
pwm_values.values[i] = pwm_value;
if (print_verbose)
warnx("Channel %d: failsafe PWM: %d", i+1, pwm_value);
}
pwm_values.channel_count++;
}
if (pwm_values.channel_count == 0) {
usage("no PWM values added");
} else {
ret = ioctl(fd, PWM_SERVO_SET_FAILSAFE_PWM, (long unsigned int)&pwm_values);
if (ret != OK)
errx(ret, "failed setting failsafe values");
}
exit(0);
} else if (!strcmp(argv[1], "test")) { } else if (!strcmp(argv[1], "test")) {
if (set_mask == 0) { if (set_mask == 0) {
@ -421,10 +451,15 @@ pwm_main(int argc, char *argv[])
err(1, "PWM_SERVO_GET_SELECT_UPDATE_RATE"); err(1, "PWM_SERVO_GET_SELECT_UPDATE_RATE");
} }
struct pwm_output_values failsafe_pwm;
struct pwm_output_values disarmed_pwm; struct pwm_output_values disarmed_pwm;
struct pwm_output_values min_pwm; struct pwm_output_values min_pwm;
struct pwm_output_values max_pwm; struct pwm_output_values max_pwm;
ret = ioctl(fd, PWM_SERVO_GET_FAILSAFE_PWM, (unsigned long)&failsafe_pwm);
if (ret != OK) {
err(1, "PWM_SERVO_GET_FAILSAFE_PWM");
}
ret = ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (unsigned long)&disarmed_pwm); ret = ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (unsigned long)&disarmed_pwm);
if (ret != OK) { if (ret != OK) {
err(1, "PWM_SERVO_GET_DISARMED_PWM"); err(1, "PWM_SERVO_GET_DISARMED_PWM");
@ -452,7 +487,8 @@ pwm_main(int argc, char *argv[])
printf(" (default rate: %d Hz", info_default_rate); printf(" (default rate: %d Hz", info_default_rate);
printf(" disarmed; %d us, min: %d us, max: %d us)", disarmed_pwm.values[i], min_pwm.values[i], max_pwm.values[i]); printf(" failsafe: %d, disarmed: %d us, min: %d us, max: %d us)",
failsafe_pwm.values[i], disarmed_pwm.values[i], min_pwm.values[i], max_pwm.values[i]);
printf("\n"); printf("\n");
} else { } else {
printf("%u: ERROR\n", i); printf("%u: ERROR\n", i);
@ -476,7 +512,7 @@ pwm_main(int argc, char *argv[])
exit(0); exit(0);
} }
usage("specify arm|disarm|rate|min|max|disarmed|test|info"); usage("specify arm|disarm|rate|failsafe|disarmed|min|max|test|info");
return 0; return 0;
} }