diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2015-02-21 11:08:08 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2015-02-23 19:50:51 +0100 |
commit | 3532a09a151f81c5a3968b62fabf7fd293504620 (patch) | |
tree | fa4a3651d5407e33d7e5b53fc9d20f18539299d9 | |
parent | 653b3e71be7059707a0963e15dfb12e5403025aa (diff) | |
download | px4-firmware-3532a09a151f81c5a3968b62fabf7fd293504620.tar.gz px4-firmware-3532a09a151f81c5a3968b62fabf7fd293504620.tar.bz2 px4-firmware-3532a09a151f81c5a3968b62fabf7fd293504620.zip |
PWM command: Fix min/max/disarmed/failsafe commands to also read the current settings first before modifying them.
-rw-r--r-- | src/systemcmds/pwm/pwm.c | 35 |
1 files changed, 27 insertions, 8 deletions
diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index f8da686cd..620b0a6f6 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -279,7 +279,14 @@ pwm_main(int argc, char *argv[]) if (pwm_value == 0) usage("no PWM value provided"); - struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0}; + struct pwm_output_values pwm_values; + memset(&pwm_values, 0, sizeof(pwm_values)); + pwm_values.channel_count = servo_count; + /* first get current state before modifying it */ + ret = ioctl(fd, PWM_SERVO_GET_MIN_PWM, (long unsigned int)&pwm_values); + if (ret != OK) { + errx(ret, "failed get min values"); + } for (unsigned i = 0; i < servo_count; i++) { if (set_mask & 1<<i) { @@ -287,7 +294,6 @@ pwm_main(int argc, char *argv[]) if (print_verbose) warnx("Channel %d: min PWM: %d", i+1, pwm_value); } - pwm_values.channel_count++; } if (pwm_values.channel_count == 0) { @@ -308,7 +314,14 @@ pwm_main(int argc, char *argv[]) if (pwm_value == 0) usage("no PWM value provided"); - struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0}; + struct pwm_output_values pwm_values; + memset(&pwm_values, 0, sizeof(pwm_values)); + pwm_values.channel_count = servo_count; + /* first get current state before modifying it */ + ret = ioctl(fd, PWM_SERVO_GET_MAX_PWM, (long unsigned int)&pwm_values); + if (ret != OK) { + errx(ret, "failed get max values"); + } for (unsigned i = 0; i < servo_count; i++) { if (set_mask & 1<<i) { @@ -316,7 +329,6 @@ pwm_main(int argc, char *argv[]) if (print_verbose) warnx("Channel %d: max PWM: %d", i+1, pwm_value); } - pwm_values.channel_count++; } if (pwm_values.channel_count == 0) { @@ -338,8 +350,10 @@ pwm_main(int argc, char *argv[]) warnx("reading disarmed value of zero, disabling disarmed PWM"); struct pwm_output_values pwm_values; + memset(&pwm_values, 0, sizeof(pwm_values)); + pwm_values.channel_count = servo_count; /* first get current state before modifying it */ - ret = ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (long unsigned int)&pwm_values.values); + ret = ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (long unsigned int)&pwm_values); if (ret != OK) { errx(ret, "failed get disarmed values"); } @@ -351,7 +365,6 @@ pwm_main(int argc, char *argv[]) warnx("chan %d: disarmed PWM: %d", i+1, pwm_value); } } - pwm_values.channel_count++; } if (pwm_values.channel_count == 0) { @@ -373,7 +386,14 @@ pwm_main(int argc, char *argv[]) if (pwm_value == 0) usage("no PWM provided"); - struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0}; + struct pwm_output_values pwm_values; + memset(&pwm_values, 0, sizeof(pwm_values)); + pwm_values.channel_count = servo_count; + /* first get current state before modifying it */ + ret = ioctl(fd, PWM_SERVO_GET_FAILSAFE_PWM, (long unsigned int)&pwm_values); + if (ret != OK) { + errx(ret, "failed get failsafe values"); + } for (unsigned i = 0; i < servo_count; i++) { if (set_mask & 1<<i) { @@ -381,7 +401,6 @@ pwm_main(int argc, char *argv[]) if (print_verbose) warnx("Channel %d: failsafe PWM: %d", i+1, pwm_value); } - pwm_values.channel_count++; } if (pwm_values.channel_count == 0) { |