void
controls_init(void)
{
/* DSM input */
dsm_init("/dev/ttyS0");
/* S.bus input */
sbus_init("/dev/ttyS2");
/* default to a 1:1 input map, all enabled */
for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) {
unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = 0;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MIN] = 1000;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_CENTER] = 1500;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MAX] = 2000;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_DEADZONE] = 30;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_ASSIGNMENT] = i;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
}
c_gather_dsm = perf_alloc(PC_ELAPSED, "c_gather_dsm");
c_gather_sbus = perf_alloc(PC_ELAPSED, "c_gather_sbus");
c_gather_ppm = perf_alloc(PC_ELAPSED, "c_gather_ppm");
}
void
controls_init(void)
{
/* no channels */
r_raw_rc_count = 0;
system_state.rc_channels_timestamp_received = 0;
system_state.rc_channels_timestamp_valid = 0;
/* DSM input (USART1) */
dsm_init("/dev/ttyS0");
/* S.bus input (USART3) */
sbus_init("/dev/ttyS2");
/* default to a 1:1 input map, all enabled */
for (unsigned i = 0; i < PX4IO_RC_INPUT_CHANNELS; i++) {
unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = 0;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MIN] = 1000;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_CENTER] = 1500;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MAX] = 2000;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_DEADZONE] = 30;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_ASSIGNMENT] = i;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
}
c_gather_dsm = perf_alloc(PC_ELAPSED, "c_gather_dsm");
c_gather_sbus = perf_alloc(PC_ELAPSED, "c_gather_sbus");
c_gather_ppm = perf_alloc(PC_ELAPSED, "c_gather_ppm");
}
int main(int argc, char *argv[])
{
int i,rc,nprocs,myrank;
int *p;
rc = dsm_init();
nprocs = dsm_nprocs();
myrank = dsm_rank();
p = dsm_malloc();
if (myrank == 0)
p[nprocs] = 0;
rc = dsm_barrier();
dsm_mutex_lock(0);
p[myrank] = myrank;
p[nprocs]++;
rc = dsm_sync();
dsm_mutex_unlock(0);
rc = dsm_barrier();
dsm_mutex_lock(1);
for (i=0; i < nprocs; i++)
printf("%d\n",p[i]);
printf("%d\n",p[nprocs]);
dsm_mutex_unlock(1);
rc = dsm_finalize();
}
int
dsm_start() {
spawn_processes();
dsm_init(proc_id);
start_server_thread();
child_process();
return 0;
}
static void
serial_rx_init(unsigned serial_mode)
{
if (serial_mode == system_state.serial_rx_mode)
return;
system_state.serial_rx_mode = serial_mode;
if (serial_mode == RX_MODE_PPM_ONLY) {
if (rx_fd != -1) {
pollcount = 1;
close(rx_fd);
rx_fd = -1;
}
return;
}
/* open the serial port used for DSM and S.bus communication */
rx_fd = open("/dev/ttyS0", O_RDONLY | O_NONBLOCK);
pollfds[1].fd = rx_fd;
pollfds[1].events = POLLIN;
pollcount = 2;
struct termios t;
tcgetattr(rx_fd, &t);
switch (serial_mode) {
case RX_MODE_DSM_10BIT:
case RX_MODE_DSM_11BIT:
/* 115200, no parity, one stop bit */
cfsetspeed(&t, 115200);
t.c_cflag &= ~(CSTOPB | PARENB);
dsm_init(serial_mode);
break;
case RX_MODE_FUTABA_SBUS:
/* 100000, even parity, two stop bits */
cfsetspeed(&t, 100000);
t.c_cflag |= (CSTOPB | PARENB);
sbus_init(serial_mode);
break;
default:
break;
}
tcsetattr(rx_fd, TCSANOW, &t);
}
int main(int argc, char *argv[])
{
ex_t rc = EX_OK;
bfpath *bfp;
bfpath_mode mode;
fBogoutil = true;
signal_setup(); /* setup to catch signals */
atexit(bf_exit);
progtype = build_progtype(progname, DB_TYPE);
set_today(); /* compute current date for token age */
process_arglist(argc, argv);
process_config_files(false, longopts_bogoutil); /* need to read lock sizes */
/* Extra or missing parameters */
if (flag != M_WORD && flag != M_LIST_LOGFILES && argc != optind) {
fprintf(stderr, "Missing or extraneous argument.\n");
usage(stderr);
exit(EX_ERROR);
}
bfp = bfpath_create(ds_file);
if (bogohome == NULL)
set_bogohome( "." ); /* set default */
bfpath_set_bogohome(bfp);
mode = get_mode(flag);
if (bfpath_check_mode(bfp, mode)) {
if (bfp->isdir)
bfpath_set_filename(bfp, WORDLIST);
}
if (!bfpath_check_mode(bfp, mode)) {
fprintf(stderr, "Can't open wordlist '%s'\n", bfp->filepath);
exit(EX_ERROR);
}
errno = 0; /* clear error status */
switch (flag) {
case M_RECOVER:
ds_init(bfp);
rc = ds_recover(bfp, false);
break;
case M_CRECOVER:
ds_init(bfp);
rc = ds_recover(bfp, true);
break;
case M_CHECKPOINT:
ds_init(bfp);
rc = ds_checkpoint(bfp);
break;
case M_LIST_LOGFILES:
dsm_init(bfp);
rc = ds_list_logfiles(bfp, argc - optind, argv + optind);
break;
case M_PURGELOGS:
ds_init(bfp);
rc = ds_purgelogs(bfp);
break;
case M_REMOVEENV:
dsm_init(bfp);
rc = ds_remove(bfp);
break;
case M_VERIFY:
dsm_init(bfp);
rc = ds_verify(bfp);
break;
case M_LEAFPAGES:
{
u_int32_t c;
dsm_init(bfp);
c = ds_leafpages(bfp);
if (c == 0xffffffff) {
fprintf(stderr, "%s: error getting leaf page count.\n", ds_file);
rc = EX_ERROR;
} else if (c == 0) {
puts("UNKNOWN");
} else {
printf("%lu\n", (unsigned long)c);
}
}
break;
case M_PAGESIZE:
{
u_int32_t s;
dsm_init(bfp);
s = ds_pagesize(bfp);
if (s == 0xffffffff) {
fprintf(stderr, "%s: error getting page size.\n", ds_file);
} else if (s == 0) {
puts("UNKNOWN");
} else {
printf("%lu\n", (unsigned long)s);
}
}
break;
case M_DUMP:
rc = dump_wordlist(bfp);
break;
case M_LOAD:
rc = load_wordlist(bfp) ? EX_ERROR : EX_OK;
break;
case M_MAINTAIN:
maintain = true;
rc = maintain_wordlist_file(bfp);
break;
case M_WORD:
argc -= optind;
argv += optind;
rc = display_words(bfp, argc, argv, prob);
break;
case M_HIST:
rc = histogram(bfp);
break;
case M_ROBX:
rc = get_robx(bfp);
break;
case M_NONE:
default:
/* should have been handled above */
abort();
break;
}
bfpath_free(bfp);
return rc;
}
void
controls_main(void)
{
struct pollfd fds[2];
/* DSM input */
fds[0].fd = dsm_init("/dev/ttyS0");
fds[0].events = POLLIN;
/* S.bus input */
fds[1].fd = sbus_init("/dev/ttyS2");
fds[1].events = POLLIN;
for (;;) {
/* run this loop at ~100Hz */
poll(fds, 2, 10);
/*
* Gather R/C control inputs from supported sources.
*
* Note that if you're silly enough to connect more than
* one control input source, they're going to fight each
* other. Don't do that.
*/
bool locked = false;
if (fds[0].revents & POLLIN)
locked |= dsm_input();
if (fds[1].revents & POLLIN)
locked |= sbus_input();
/*
* If we don't have lock from one of the serial receivers,
* look for PPM. It shares an input with S.bus, so there's
* a possibility it will mis-parse an S.bus frame.
*
* XXX each S.bus frame will cause a PPM decoder interrupt
* storm (lots of edges). It might be sensible to actually
* disable the PPM decoder completely if we have an alternate
* receiver lock.
*/
if (!locked)
ppm_input();
/*
* If we haven't seen any new control data in 200ms, assume we
* have lost input and tell FMU.
*/
if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) {
/* set the number of channels to zero - no inputs */
system_state.rc_channels = 0;
/* trigger an immediate report to the FMU */
system_state.fmu_report_due = true;
}
/* XXX do bypass mode, etc. here */
/* do PWM output updates */
mixer_tick();
}
}
