forked from Archive/PX4-Autopilot
Made pwm command sending continously, improved failsafe logic
This commit is contained in:
parent
aa641b5c34
commit
8567134d64
|
@ -98,7 +98,7 @@ mixer_tick(void)
|
|||
if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
|
||||
isr_debug(1, "AP RX timeout");
|
||||
}
|
||||
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM);
|
||||
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK);
|
||||
r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;
|
||||
|
||||
} else {
|
||||
|
@ -112,12 +112,11 @@ mixer_tick(void)
|
|||
* Decide which set of controls we're using.
|
||||
*/
|
||||
|
||||
/* do not mix if mixer is invalid or if RAW_PWM mode is on and FMU is good */
|
||||
/* do not mix if RAW_PWM mode is on and FMU is good */
|
||||
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
|
||||
!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
|
||||
(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
|
||||
|
||||
/* don't actually mix anything - we already have raw PWM values or
|
||||
not a valid mixer. */
|
||||
/* don't actually mix anything - we already have raw PWM values */
|
||||
source = MIX_NONE;
|
||||
|
||||
} else {
|
||||
|
@ -196,10 +195,9 @@ mixer_tick(void)
|
|||
bool should_arm = (
|
||||
/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
|
||||
/* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
|
||||
/* there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) &&
|
||||
/* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) &&
|
||||
/* FMU is available or FMU is not available but override is an option */
|
||||
((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ))
|
||||
/* there is valid input via direct PWM or mixer */ ((r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) ||
|
||||
/* or failsafe was set manually */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)) &&
|
||||
/* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
|
||||
);
|
||||
|
||||
if (should_arm && !mixer_servos_armed) {
|
||||
|
|
|
@ -180,7 +180,8 @@ pwm_main(int argc, char *argv[])
|
|||
}
|
||||
|
||||
/* iterate remaining arguments */
|
||||
unsigned channel = 0;
|
||||
unsigned nchannels = 0;
|
||||
unsigned channel[8] = {0};
|
||||
while (argc--) {
|
||||
const char *arg = argv[0];
|
||||
argv++;
|
||||
|
@ -204,13 +205,15 @@ pwm_main(int argc, char *argv[])
|
|||
}
|
||||
unsigned pwm_value = strtol(arg, &ep, 0);
|
||||
if (*ep == '\0') {
|
||||
ret = ioctl(fd, PWM_SERVO_SET(channel), pwm_value);
|
||||
if (ret != OK)
|
||||
err(1, "PWM_SERVO_SET(%d)", channel);
|
||||
channel++;
|
||||
channel[nchannels] = pwm_value;
|
||||
nchannels++;
|
||||
|
||||
if (nchannels >= sizeof(channel) / sizeof(channel[0]))
|
||||
err(1, "too many pwm values (max %d)", sizeof(channel) / sizeof(channel[0]));
|
||||
|
||||
continue;
|
||||
}
|
||||
usage("unrecognised option");
|
||||
usage("unrecognized option");
|
||||
}
|
||||
|
||||
/* print verbose info */
|
||||
|
@ -250,5 +253,38 @@ pwm_main(int argc, char *argv[])
|
|||
}
|
||||
fflush(stdout);
|
||||
}
|
||||
|
||||
/* perform PWM output */
|
||||
if (nchannels) {
|
||||
|
||||
/* Open console directly to grab CTRL-C signal */
|
||||
int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
|
||||
if (!console)
|
||||
err(1, "failed opening console");
|
||||
|
||||
warnx("Press CTRL-C or 'c' to abort.");
|
||||
|
||||
while (1) {
|
||||
for (int i = 0; i < nchannels; i++) {
|
||||
ret = ioctl(fd, PWM_SERVO_SET(i), channel[i]);
|
||||
if (ret != OK)
|
||||
err(1, "PWM_SERVO_SET(%d)", i);
|
||||
}
|
||||
|
||||
/* abort on user request */
|
||||
char c;
|
||||
if (read(console, &c, 1) == 1) {
|
||||
if (c == 0x03 || c == 0x63) {
|
||||
warnx("User abort\n");
|
||||
close(console);
|
||||
exit(0);
|
||||
}
|
||||
}
|
||||
|
||||
/* rate limit to ~ 20 Hz */
|
||||
usleep(50000);
|
||||
}
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
Loading…
Reference in New Issue