fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

Exported disarmed PWM values as IOCTLs

Browse Source
This commit is contained in:
Lorenz Meier 2013-09-10 21:35:50 +02:00
parent 373a74adb9
commit 8131d28a0f
2 changed files with 75 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/drivers/drv_pwm_output.h
View File

@ -79,6 +79,7 @@ typedef uint16_t servo_position_t;
struct pwm_output_values {
/** desired pulse widths for each of the supported channels */
servo_position_t values[PWM_OUTPUT_MAX_CHANNELS];
int channel_count;
};
/*
@ -118,9 +119,18 @@ ORB_DECLARE(output_pwm);
/** start DSM bind */
#define DSM_BIND_START _IOC(_PWM_SERVO_BASE, 7)
/** Power up DSM receiver */
/** power up DSM receiver */
#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 8)
/** set the PWM value when disarmed - should be no PWM (zero) by default */
#define PWM_SERVO_SET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 9)
/** set the minimum PWM value the output will send */
#define PWM_SERVO_SET_MIN_PWM _IOC(_PWM_SERVO_BASE, 10)
/** set the maximum PWM value the output will send */
#define PWM_SERVO_SET_MAX_PWM _IOC(_PWM_SERVO_BASE, 11)
/** set a single servo to a specific value */
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)

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

@ -177,21 +177,6 @@ public:
*/
int set_failsafe_values(const uint16_t *vals, unsigned len);
/**
* Set the minimum PWM signals when armed
*/
int set_min_values(const uint16_t *vals, unsigned len);
/**
* Set the maximum PWM signal when armed
*/
int set_max_values(const uint16_t *vals, unsigned len);
/**
* Set an idle PWM signal that is active right after startup, even when SAFETY_SAFE
*/
int set_idle_values(const uint16_t *vals, unsigned len);
/**
* Print IO status.
*
@ -391,6 +376,21 @@ private:
*/
int mixer_send(const char *buf, unsigned buflen);
/**
* Set the minimum PWM signals when armed
*/
int set_min_values(const uint16_t *vals, unsigned len);
/**
* Set the maximum PWM signal when armed
*/
int set_max_values(const uint16_t *vals, unsigned len);
/**
* Set an idle PWM signal that is active right after startup, even when SAFETY_SAFE
*/
int set_idle_values(const uint16_t *vals, unsigned len);
/**
* Handle a status update from IO.
*
@ -1674,6 +1674,24 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
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);
}
break;
case PWM_SERVO_SET_MIN_PWM: {
struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
set_min_values(pwm->values, pwm->channel_count);
}
break;
case PWM_SERVO_SET_MAX_PWM: {
struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
set_max_values(pwm->values, pwm->channel_count);
}
break;
case PWM_SERVO_GET_COUNT:
*(unsigned *)arg = _max_actuators;
break;
@ -2265,26 +2283,26 @@ px4io_main(int argc, char *argv[])
errx(1, "min command needs at least one channel value (PWM)");
}
if (g_dev != nullptr) {
int iofd = open(PWM_OUTPUT_DEVICE_PATH, 0);
struct pwm_output_values pwm;
/* set values for first 8 channels, fill unassigned channels with 900. */
uint16_t min[8];
if (iofd > 0) {
for (unsigned i = 0; i < sizeof(min) / sizeof(min[0]); i++)
pwm.channel_count = 0;
for (unsigned i = 0; i < sizeof(pwm.values) / sizeof(pwm.values[0]); i++)
{
/* set channel to commanline argument or to 900 for non-provided channels */
if (argc > i + 2) {
min[i] = atoi(argv[i+2]);
if (min[i] < 900 || min[i] > 1200) {
pwm.values[i] = atoi(argv[i+2]);
if (pwm.values[i] < 900 || pwm.values[i] > 1200) {
errx(1, "value out of range of 900 < value < 1200. Aborting.");
}
} else {
/* a zero value will the default */
min[i] = 0;
pwm.channel_count++;
}
}
int ret = g_dev->set_min_values(min, sizeof(min) / sizeof(min[0]));
int ret = ioctl(iofd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm);
if (ret != OK)
errx(ret, "failed setting min values");
@ -2300,26 +2318,26 @@ px4io_main(int argc, char *argv[])
errx(1, "max command needs at least one channel value (PWM)");
}
if (g_dev != nullptr) {
int iofd = open(PWM_OUTPUT_DEVICE_PATH, 0);
struct pwm_output_values pwm;
/* set values for first 8 channels, fill unassigned channels with 2100. */
uint16_t max[8];
if (iofd > 0) {
for (unsigned i = 0; i < sizeof(max) / sizeof(max[0]); i++)
pwm.channel_count = 0;
for (int i = 0; i < sizeof(pwm.values) / sizeof(pwm.values[0]); i++)
{
/* set channel to commanline argument or to 2100 for non-provided channels */
if (argc > i + 2) {
max[i] = atoi(argv[i+2]);
if (max[i] < 1800 || max[i] > 2100) {
pwm.values[i] = atoi(argv[i+2]);
if (pwm.values[i] < 1800 || pwm.values[i] > 2100) {
errx(1, "value out of range of 1800 < value < 2100. Aborting.");
}
} else {
/* a zero value will the default */
max[i] = 0;
pwm.channel_count++;
}
}
int ret = g_dev->set_max_values(max, sizeof(max) / sizeof(max[0]));
int ret = ioctl(iofd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm);
if (ret != OK)
errx(ret, "failed setting max values");
@ -2329,32 +2347,32 @@ px4io_main(int argc, char *argv[])
exit(0);
}
if (!strcmp(argv[1], "idle")) {
if (!strcmp(argv[1], "idle") || !strcmp(argv[1], "disarmed")) {
if (argc < 3) {
errx(1, "max command needs at least one channel value (PWM)");
}
if (g_dev != nullptr) {
int iofd = open(PWM_OUTPUT_DEVICE_PATH, 0);
struct pwm_output_values pwm;
/* set values for first 8 channels, fill unassigned channels with 0. */
uint16_t idle[8];
if (iofd > 0) {
for (unsigned i = 0; i < sizeof(idle) / sizeof(idle[0]); i++)
pwm.channel_count = 0;
for (unsigned i = 0; i < sizeof(pwm.values) / sizeof(pwm.values[0]); i++)
{
/* set channel to commanline argument or to 0 for non-provided channels */
if (argc > i + 2) {
idle[i] = atoi(argv[i+2]);
if (idle[i] < 900 || idle[i] > 2100) {
pwm.values[i] = atoi(argv[i+2]);
if (pwm.values[i] < 900 || pwm.values[i] > 2100) {
errx(1, "value out of range of 900 < value < 2100. Aborting.");
}
} else {
/* a zero value will the default */
idle[i] = 0;
pwm.channel_count++;
}
}
int ret = g_dev->set_idle_values(idle, sizeof(idle) / sizeof(idle[0]));
int ret = ioctl(iofd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm);
if (ret != OK)
errx(ret, "failed setting idle values");