void Pod::watch() {
watcher_t w = create_watcher();
shm_watch(switches, w);
shm_watch(mission_start_switch, w);
shm_watch(merge_status, w);
// Initial write:
// TODO: resync before this?
sendInt16(30, mission_start_switch.mission_light);
while (isActive()) {
wait_watcher(w, false); // Don't block if there has been an update
// we want to make sure it gets through
shm_getg(switches, switches_new);
shm_getg(mission_start_switch, mission_start_switch_new);
shm_getg(merge_status, merge_status_new);
if (mission_start_switch_new.mission_light != mission_start_switch.mission_light) {
sendInt16(30, (mission_start_switch_new.mission_light));
mission_start_switch.mission_light = mission_start_switch_new.mission_light;
}
}
destroy_watcher(w);
}
void Him::watch() {
watcher_t w = create_watcher();
shm_watch(him_settings, w);
shm_watch(him, w);
// Initial write:
// TODO: resync before this?
sendFloat(16, him_settings.heading_offset);
sendFloat(18, him_settings.pitchoffset);
sendFloat(20, him_settings.rolloffset);
sendFloat(22, him_settings.xacceloffset);
sendFloat(24, him_settings.yacceloffset);
sendFloat(26, him_settings.zacceloffset);
sendInt16(28, him_settings.xcompoffset);
sendInt16(29, him_settings.ycompoffset);
sendInt16(30, him_settings.zcompoffset);
while (isActive()) {
wait_watcher(w, false); // Don't block if there has been an update
// we want to make sure it gets through
shm_getg(him_settings, him_settings_new);
shm_getg(him, him_new);
if (him_settings_new.heading_offset != him_settings.heading_offset) {
sendFloat(16, (him_settings_new.heading_offset));
him_settings.heading_offset = him_settings_new.heading_offset;
}
if (him_settings_new.pitchoffset != him_settings.pitchoffset) {
sendFloat(18, (him_settings_new.pitchoffset));
him_settings.pitchoffset = him_settings_new.pitchoffset;
}
if (him_settings_new.rolloffset != him_settings.rolloffset) {
sendFloat(20, (him_settings_new.rolloffset));
him_settings.rolloffset = him_settings_new.rolloffset;
}
if (him_settings_new.xacceloffset != him_settings.xacceloffset) {
sendFloat(22, (him_settings_new.xacceloffset));
him_settings.xacceloffset = him_settings_new.xacceloffset;
}
if (him_settings_new.yacceloffset != him_settings.yacceloffset) {
sendFloat(24, (him_settings_new.yacceloffset));
him_settings.yacceloffset = him_settings_new.yacceloffset;
}
if (him_settings_new.zacceloffset != him_settings.zacceloffset) {
sendFloat(26, (him_settings_new.zacceloffset));
him_settings.zacceloffset = him_settings_new.zacceloffset;
}
if (him_settings_new.xcompoffset != him_settings.xcompoffset) {
sendInt16(28, (him_settings_new.xcompoffset));
him_settings.xcompoffset = him_settings_new.xcompoffset;
}
if (him_settings_new.ycompoffset != him_settings.ycompoffset) {
sendInt16(29, (him_settings_new.ycompoffset));
him_settings.ycompoffset = him_settings_new.ycompoffset;
}
if (him_settings_new.zcompoffset != him_settings.zcompoffset) {
sendInt16(30, (him_settings_new.zcompoffset));
him_settings.zcompoffset = him_settings_new.zcompoffset;
}
}
destroy_watcher(w);
}
void Actuators::watch() {
watcher_t w = create_watcher();
shm_watch(switches, w);
shm_watch(actuator_desires, w);
shm_watch(actuator_status, w);
// Initial write:
// TODO: resync before this?
sendInt16(16, actuator_desires.trigger_01);
sendInt16(17, actuator_desires.trigger_02);
sendInt16(18, actuator_desires.trigger_03);
sendInt16(19, actuator_desires.trigger_04);
sendInt16(20, actuator_desires.trigger_05);
sendInt16(21, actuator_desires.trigger_06);
sendInt16(22, actuator_desires.trigger_07);
sendInt16(23, actuator_desires.trigger_08);
sendInt16(24, actuator_desires.trigger_09);
sendInt16(25, actuator_desires.trigger_10);
sendInt16(26, actuator_desires.trigger_11);
sendInt16(27, actuator_desires.trigger_12);
sendInt16(28, actuator_desires.trigger_13);
sendInt16(29, actuator_desires.trigger_14);
sendInt16(30, actuator_desires.motor_pwm_1);
sendInt16(31, actuator_desires.motor_pwm_2);
sendInt16(32, switches.soft_kill);
while (isActive()) {
wait_watcher(w, false); // Don't block if there has been an update
// we want to make sure it gets through
shm_getg(switches, switches_new);
shm_getg(actuator_desires, actuator_desires_new);
shm_getg(actuator_status, actuator_status_new);
if (actuator_desires_new.trigger_01 != actuator_desires.trigger_01) {
sendInt16(16, (actuator_desires_new.trigger_01));
actuator_desires.trigger_01 = actuator_desires_new.trigger_01;
}
if (actuator_desires_new.trigger_02 != actuator_desires.trigger_02) {
sendInt16(17, (actuator_desires_new.trigger_02));
actuator_desires.trigger_02 = actuator_desires_new.trigger_02;
}
if (actuator_desires_new.trigger_03 != actuator_desires.trigger_03) {
sendInt16(18, (actuator_desires_new.trigger_03));
actuator_desires.trigger_03 = actuator_desires_new.trigger_03;
}
if (actuator_desires_new.trigger_04 != actuator_desires.trigger_04) {
sendInt16(19, (actuator_desires_new.trigger_04));
actuator_desires.trigger_04 = actuator_desires_new.trigger_04;
}
if (actuator_desires_new.trigger_05 != actuator_desires.trigger_05) {
sendInt16(20, (actuator_desires_new.trigger_05));
actuator_desires.trigger_05 = actuator_desires_new.trigger_05;
}
if (actuator_desires_new.trigger_06 != actuator_desires.trigger_06) {
sendInt16(21, (actuator_desires_new.trigger_06));
actuator_desires.trigger_06 = actuator_desires_new.trigger_06;
}
if (actuator_desires_new.trigger_07 != actuator_desires.trigger_07) {
sendInt16(22, (actuator_desires_new.trigger_07));
actuator_desires.trigger_07 = actuator_desires_new.trigger_07;
}
if (actuator_desires_new.trigger_08 != actuator_desires.trigger_08) {
sendInt16(23, (actuator_desires_new.trigger_08));
actuator_desires.trigger_08 = actuator_desires_new.trigger_08;
}
if (actuator_desires_new.trigger_09 != actuator_desires.trigger_09) {
sendInt16(24, (actuator_desires_new.trigger_09));
actuator_desires.trigger_09 = actuator_desires_new.trigger_09;
}
if (actuator_desires_new.trigger_10 != actuator_desires.trigger_10) {
sendInt16(25, (actuator_desires_new.trigger_10));
actuator_desires.trigger_10 = actuator_desires_new.trigger_10;
}
if (actuator_desires_new.trigger_11 != actuator_desires.trigger_11) {
sendInt16(26, (actuator_desires_new.trigger_11));
actuator_desires.trigger_11 = actuator_desires_new.trigger_11;
}
if (actuator_desires_new.trigger_12 != actuator_desires.trigger_12) {
sendInt16(27, (actuator_desires_new.trigger_12));
actuator_desires.trigger_12 = actuator_desires_new.trigger_12;
}
if (actuator_desires_new.trigger_13 != actuator_desires.trigger_13) {
sendInt16(28, (actuator_desires_new.trigger_13));
actuator_desires.trigger_13 = actuator_desires_new.trigger_13;
}
if (actuator_desires_new.trigger_14 != actuator_desires.trigger_14) {
sendInt16(29, (actuator_desires_new.trigger_14));
actuator_desires.trigger_14 = actuator_desires_new.trigger_14;
}
if (actuator_desires_new.motor_pwm_1 != actuator_desires.motor_pwm_1) {
sendInt16(30, (actuator_desires_new.motor_pwm_1));
actuator_desires.motor_pwm_1 = actuator_desires_new.motor_pwm_1;
}
if (actuator_desires_new.motor_pwm_2 != actuator_desires.motor_pwm_2) {
sendInt16(31, (actuator_desires_new.motor_pwm_2));
actuator_desires.motor_pwm_2 = actuator_desires_new.motor_pwm_2;
}
if (switches_new.soft_kill != switches.soft_kill) {
sendInt16(32, (switches_new.soft_kill));
switches.soft_kill = switches_new.soft_kill;
}
}
destroy_watcher(w);
}
static int create_ns(const char *statedir, const char *name)
{
char str[64];
uid_t uid = getuid();
gid_t gid = getgid();
if (unshare(CLONE_NEWNS | CLONE_NEWNET |
CLONE_NEWUTS | CLONE_NEWUSER) < 0)
pdie("can't unshare namespaces");
if (access("/proc/self/setgroups", O_RDONLY) == 0)
write_file("/proc/self/setgroups", "deny");
snprintf(str, sizeof(str), "0 %d 1", uid);
write_file("/proc/self/uid_map", str);
snprintf(str, sizeof(str), "0 %d 1", gid);
write_file("/proc/self/gid_map", str);
if (sethostname(name, strlen(name)) < 0)
pdie("can't set hostname");
setup_ipv4("lo", "127.0.0.1", "255.0.0.0", false, 0);
mkdir(statedir, 0755);
char *local_etc = populate_statedir(statedir, "etc", true);
char *workdir = populate_statedir(statedir, "workdir", true);
char *resolv = populate_statedir(statedir, "etc/resolv.conf", false);
char *mount_opts;
if (asprintf(&mount_opts, "lowerdir=/etc,upperdir=%s,workdir=%s",
local_etc, workdir) < 0) {
die("can't allocate memory\n");
}
/*
* overlayfs is only usable on patched kernels (e.g. Ubuntu) due to
* permission checks, but it is the cleanest solution because it
* overrides symlinks. If we have to use bind mounts instead,
* tell the watcher process to re-create the bind mount if
* resolv.conf gets deleted.
*/
int inotify_fd = -1, inotify_wd = -1;
if (mount("overlay", "/etc", "overlay", 0, mount_opts) == 0) {
/* pass through */
} else {
inotify_fd = inotify_init();
if (inotify_fd < 0)
pdie("can't create inotify socket");
inotify_wd = watch_and_bind_mount(inotify_fd, resolv);
if (inotify_wd < 0)
die("can't watch resolv.conf\n");
}
/*
* Create the initial watcher connection here so that the parent
* process doesn't need to wait for the child process to start up.
*/
int initial_conn_fd[2];
if (socketpair(AF_UNIX, SOCK_STREAM, 0, initial_conn_fd) < 0)
pdie("socketpair failed");
write_pid(statedir, create_watcher(statedir, initial_conn_fd[1],
resolv, inotify_fd, inotify_wd));
close(initial_conn_fd[1]);
close(inotify_fd);
free(mount_opts);
free(resolv);
free(workdir);
free(local_etc);
return initial_conn_fd[0];
}
