aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/drivers/drv_pwm_output.h18
-rw-r--r--src/drivers/px4fmu/fmu.cpp202
-rw-r--r--src/drivers/px4io/px4io.cpp128
-rw-r--r--src/modules/px4iofirmware/registers.c13
-rw-r--r--src/systemcmds/pwm/pwm.c46
5 files changed, 233 insertions, 174 deletions
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index 3357e67a5..f0dd713b2 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -154,23 +154,29 @@ ORB_DECLARE(output_pwm);
/** power up DSM receiver */
#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 */
-#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 */
-#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 */
-#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 */
-#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 */
-#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 */
-#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 */
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index e729612cc..b7b2f7b33 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -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
* modification, are permitted provided that the following conditions
@@ -78,6 +78,12 @@
# include <systemlib/ppm_decode.h>
#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
{
public:
@@ -130,9 +136,11 @@ private:
actuator_controls_s _controls;
pwm_limit_t _pwm_limit;
+ uint16_t _failsafe_pwm[_max_actuators];
uint16_t _disarmed_pwm[_max_actuators];
uint16_t _min_pwm[_max_actuators];
uint16_t _max_pwm[_max_actuators];
+ unsigned _num_failsafe_set;
unsigned _num_disarmed_set;
static void task_main_trampoline(int argc, char *argv[]);
@@ -218,7 +226,9 @@ PX4FMU::PX4FMU() :
_armed(false),
_pwm_on(false),
_mixers(nullptr),
+ _failsafe_pwm({0}),
_disarmed_pwm({0}),
+ _num_failsafe_set(0),
_num_disarmed_set(0)
{
for (unsigned i = 0; i < _max_actuators; i++) {
@@ -515,98 +525,103 @@ PX4FMU::task_main()
/* sleep waiting for data, stopping to check for PPM
* 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... */
if (ret < 0) {
log("poll error %d", errno);
usleep(1000000);
continue;
- }
+ } else if (ret == 0) {
+ /* timeout: no control data, switch to failsafe values */
+// warnx("no PWM: failsafe");
- /* do we have a control update? */
- if (fds[0].revents & POLLIN) {
-
- /* get controls - must always do this to avoid spinning */
- orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
-
- /* can we mix? */
- if (_mixers != nullptr) {
-
- unsigned num_outputs;
-
- switch (_mode) {
- 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;
- }
+ } else {
- /* 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;
+ /* do we have a control update? */
+ if (fds[0].revents & POLLIN) {
+
+ /* get controls - must always do this to avoid spinning */
+ orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
+
+ /* can we mix? */
+ if (_mixers != nullptr) {
+
+ unsigned num_outputs;
+
+ switch (_mode) {
+ 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;
}
- }
- uint16_t pwm_limited[num_outputs];
+ /* 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;
+ }
+ }
- pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);
+ uint16_t pwm_limited[num_outputs];
- /* 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);
+ pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);
- /* output to the servos */
- for (unsigned i = 0; i < num_outputs; i++) {
- up_pwm_servo_set(i, pwm_limited[i]);
- }
+ /* 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);
- /* 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);
+ /* 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? */
- if (fds[1].revents & POLLIN) {
- actuator_armed_s aa;
+ /* how about an arming update? */
+ if (fds[1].revents & POLLIN) {
+ actuator_armed_s aa;
- /* get new value */
- orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa);
+ /* get new value */
+ orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa);
- /* update the armed status and check that we're not locked down */
- bool set_armed = aa.armed && !aa.lockdown;
- if (_armed != set_armed)
- _armed = set_armed;
+ /* update the armed status and check that we're not locked down */
+ bool set_armed = aa.armed && !aa.lockdown;
+ if (_armed != set_armed)
+ _armed = set_armed;
- /* update PWM status if armed or if disarmed PWM values are set */
- bool pwm_on = (aa.armed || _num_disarmed_set > 0);
- if (_pwm_on != pwm_on) {
- _pwm_on = pwm_on;
- up_pwm_servo_arm(pwm_on);
+ /* update PWM status if armed or if disarmed PWM values are set */
+ bool pwm_on = (aa.armed || _num_disarmed_set > 0);
+ if (_pwm_on != pwm_on) {
+ _pwm_on = 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;
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: {
struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
/* discard if too many values are sent */
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index d709f1cc8..b603ffc8d 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -943,53 +943,6 @@ PX4IO::io_set_control_state()
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
PX4IO::io_set_arming_state()
@@ -1803,7 +1756,7 @@ PX4IO::print_status()
printf("failsafe");
for (unsigned i = 0; i < _max_actuators; 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++)
printf(" %u", io_reg_get(PX4IO_PAGE_DISARMED_PWM, i));
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);
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: {
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;
}
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) {
ret = -EIO;
}
@@ -1890,13 +1867,18 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
case PWM_SERVO_SET_MIN_PWM: {
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;
}
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) {
ret = -EIO;
}
@@ -1904,13 +1886,18 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
case PWM_SERVO_SET_MAX_PWM: {
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;
}
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) {
ret = -EIO;
}
@@ -2518,37 +2505,6 @@ px4io_main(int argc, char *argv[])
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")) {
@@ -2616,5 +2572,5 @@ px4io_main(int argc, char *argv[])
bind(argc, argv);
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'");
}
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index a338afe16..6a0532bee 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -199,7 +199,7 @@ uint16_t r_page_rc_input_config[PX4IO_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRI
*
* 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
@@ -276,8 +276,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
/* copy channel data */
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
- /* XXX range-check value? */
- r_page_servo_failsafe[offset] = *values;
+ if (*values == 0) {
+ /* 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 */
r_setup_arming |= PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM;
diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c
index 25f8c4e75..3185e4371 100644
--- a/src/systemcmds/pwm/pwm.c
+++ b/src/systemcmds/pwm/pwm.c
@@ -72,20 +72,21 @@ usage(const char *reason)
warnx("%s", reason);
errx(1,
"usage:\n"
- "pwm arm|disarm|rate|min|max|disarmed|test|info ...\n"
+ "pwm arm|disarm|rate|failsafe|disarmed|min|max|test|info ...\n"
"\n"
" arm Arm output\n"
" disarm Disarm output\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"
" [-m <chanmask> ] Directly supply channel mask\n"
" [-a] Configure all outputs\n"
" -r <alt_rate> PWM rate (50 to 400 Hz)\n"
"\n"
+ " failsafe ... Configure failsafe PWM values\n"
+ " disarmed ... Configure disarmed PWM values\n"
" min ... Configure minimum PWM values\n"
" max ... Configure maximum PWM values\n"
- " disarmed ... Configure disarmed PWM values\n"
" [-c <channels>] Supply channels (e.g. 1234)\n"
" [-m <chanmask> ] Directly supply channel mask (e.g. 0xF)\n"
" [-a] Configure all outputs\n"
@@ -357,6 +358,35 @@ pwm_main(int argc, char *argv[])
}
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")) {
if (set_mask == 0) {
@@ -421,10 +451,15 @@ pwm_main(int argc, char *argv[])
err(1, "PWM_SERVO_GET_SELECT_UPDATE_RATE");
}
+ struct pwm_output_values failsafe_pwm;
struct pwm_output_values disarmed_pwm;
struct pwm_output_values min_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);
if (ret != OK) {
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(" 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");
} else {
printf("%u: ERROR\n", i);
@@ -476,7 +512,7 @@ pwm_main(int argc, char *argv[])
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;
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-06 09:54:46 +0000