aboutsummaryrefslogtreecommitdiff
path: root/src/drivers
diff options
context:
space:
mode:
Diffstat (limited to 'src/drivers')
-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
3 files changed, 182 insertions, 166 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'");
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-11 03:58:20 +0000