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

[PX4IO/PWM driver] Added trim values to the PWM output drivers

Browse Source
This commit is contained in:
Bartosz Wawrzacz
2016-07-06 13:50:08 +02:00
committed by Lorenz Meier
parent 4494182bfc
commit 619efa7b45
12 changed files with 222 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/systemcmds/pwm/pwm.c
+54 -2
View File
@@ -90,6 +90,7 @@ usage(const char *reason)
"disarmed ...\t\t\tDisarmed PWM\n"
"min ...\t\t\t\tMinimum PWM\n"
"max ...\t\t\t\tMaximum PWM\n"
"trim ...\t\t\tTrim PWM\n"
"\t[-c <channels>]\t\t(e.g. 1234)\n"
"\t[-m <channel mask> ]\t(e.g. 0xF)\n"
"\t[-a]\t\t\tConfigure all outputs\n"
@@ -448,6 +449,49 @@ pwm_main(int argc, char *argv[])
exit(0);
} else if (!strcmp(argv[1], "trim")) {
if (set_mask == 0) {
usage("no channels set");
}
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_TRIM_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {
errx(ret, "failed get trim values");
}
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: trim PWM: %d", i + 1, pwm_value);
}
}
}
if (pwm_values.channel_count == 0) {
usage("no PWM values added");
} else {
ret = ioctl(fd, PWM_SERVO_SET_TRIM_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {
errx(ret, "failed setting trim values");
}
}
exit(0);
} else if (!strcmp(argv[1], "disarmed")) {
if (set_mask == 0) {
@@ -768,6 +812,8 @@ pwm_main(int argc, char *argv[])
struct pwm_output_values max_pwm;
struct pwm_output_values trim_pwm;
ret = ioctl(fd, PWM_SERVO_GET_FAILSAFE_PWM, (unsigned long)&failsafe_pwm);
if (ret != OK) {
@@ -792,6 +838,12 @@ pwm_main(int argc, char *argv[])
err(1, "PWM_SERVO_GET_MAX_PWM");
}
ret = ioctl(fd, PWM_SERVO_GET_TRIM_PWM, (unsigned long)&trim_pwm);
if (ret != OK) {
err(1, "PWM_SERVO_GET_TRIM_PWM");
}
/* print current servo values */
for (unsigned i = 0; i < servo_count; i++) {
servo_position_t spos;
@@ -809,8 +861,8 @@ pwm_main(int argc, char *argv[])
}
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(" failsafe: %d, disarmed: %d us, min: %d us, max: %d us, trim: %d us)",
failsafe_pwm.values[i], disarmed_pwm.values[i], min_pwm.values[i], max_pwm.values[i], trim_pwm.values[i]);
printf("\n");
} else {
src/systemcmds/tests/test_mixer.cpp
+8 -8
View File
@@ -87,6 +87,7 @@ int test_mixer(int argc, char *argv[])
uint16_t r_page_servo_disarmed[output_max];
uint16_t r_page_servo_control_min[output_max];
uint16_t r_page_servo_control_max[output_max];
uint16_t r_page_servo_control_trim[output_max];
uint16_t r_page_servos[output_max];
uint16_t servo_predicted[output_max];
int16_t reverse_pwm_mask = 0;
@@ -200,6 +201,7 @@ int test_mixer(int argc, char *argv[])
r_page_servo_disarmed[i] = PWM_MOTOR_OFF;
r_page_servo_control_min[i] = PWM_DEFAULT_MIN;
r_page_servo_control_max[i] = PWM_DEFAULT_MAX;
r_page_servo_control_trim[i] = PWM_DEFAULT_TRIM;
}
//PX4_INFO("PRE-ARM TEST: DISABLING SAFETY");
@@ -209,7 +211,7 @@ int test_mixer(int argc, char *argv[])
mixed = mixer_group.mix(&outputs[0], output_max, NULL);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
r_page_servo_control_max, r_page_servo_control_trim, outputs, r_page_servos, &pwm_limit);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) {
@@ -250,7 +252,7 @@ int test_mixer(int argc, char *argv[])
mixed = mixer_group.mix(&outputs[0], output_max, NULL);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
r_page_servo_control_max, r_page_servo_control_trim, outputs, r_page_servos, &pwm_limit);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) {
@@ -288,14 +290,14 @@ int test_mixer(int argc, char *argv[])
r_page_servo_disarmed[i] = PWM_LOWEST_MIN;
r_page_servo_control_min[i] = PWM_DEFAULT_MIN;
r_page_servo_control_max[i] = PWM_DEFAULT_MAX;
r_page_servo_control_trim[i] = PWM_DEFAULT_TRIM;
}
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max, NULL);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs,
r_page_servos, &pwm_limit);
r_page_servo_control_max, r_page_servo_control_trim, outputs, r_page_servos, &pwm_limit);
//fprintf(stderr, "mixed %d outputs (max %d)", mixed, output_max);
@@ -323,8 +325,7 @@ int test_mixer(int argc, char *argv[])
mixed = mixer_group.mix(&outputs[0], output_max, NULL);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs,
r_page_servos, &pwm_limit);
r_page_servo_control_max, r_page_servo_control_trim, outputs, r_page_servos, &pwm_limit);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) {
@@ -361,8 +362,7 @@ int test_mixer(int argc, char *argv[])
mixed = mixer_group.mix(&outputs[0], output_max, NULL);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs,
r_page_servos, &pwm_limit);
r_page_servo_control_max, r_page_servo_control_trim, outputs, r_page_servos, &pwm_limit);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) {