static int enable_disable_cm(struct sensors_control_context_t *dev, uint32_t active, uint32_t sensors, uint32_t mask) { int rc = 0; uint32_t now_active_cm_sensors; int fd = open_cm(dev); if (fd < 0) { LOGE("Couldn't open %s (%s)", CM_DEVICE_NAME, strerror(errno)); return 0; } LOGV("(before) cm sensors = %08x, real = %08x", sensors, read_cm_sensors_state(fd)); if (mask & SENSORS_CM_PROXIMITY) { int flags = (sensors & SENSORS_CM_PROXIMITY) ? 1 : 0; rc = ioctl(fd, CAPELLA_CM3602_IOCTL_ENABLE, &flags); if (rc < 0) LOGE("CAPELLA_CM3602_IOCTL_ENABLE error (%s)", strerror(errno)); } now_active_cm_sensors = read_cm_sensors_state(fd); LOGV("(after) cm sensors = %08x, real = %08x", sensors, now_active_cm_sensors); return now_active_cm_sensors; }
static int enable_disable_cm(struct sensors_control_context_t *dev, uint32_t active, uint32_t sensors, uint32_t mask) { int rc = 0; uint32_t now_active_cm_sensors; int fd = open_cm(dev); if (fd < 0) { LOGE("Couldn't open %s (%s)", CM_DEVICE_NAME, strerror(errno)); return 0; } LOGV("(before) cm sensors = %08x, real = %08x", sensors, read_cm_sensors_state(fd,0)); int flags = 0; if (mask & SENSORS_CM_PROXIMITY) { flags = (sensors & SENSORS_CM_PROXIMITY) ? 1 : 0; if(flags == 1) { uint16_t mode = 1; rc = ioctl(fd, P_IOC_POWERUP_SET_MODE, &mode); if (rc < 0) LOGE("P_IOC_POWERUP_SET_MODE error (%s)", strerror(errno)); } else { rc = ioctl(fd, P_IOC_SHUTDOWN); if (rc < 0) LOGE("P_IOC_POWERUP_SET_MODE error (%s)", strerror(errno)); } } now_active_cm_sensors = read_cm_sensors_state(fd,flags); LOGV("(after) cm sensors = %08x, real = %08x", sensors, now_active_cm_sensors); return now_active_cm_sensors; }
void* azelComm(void* arg) { int ser_attempts = 0; // number of attempts to open port int init_attempts = 0; // number of attempts to initialize motor nameThread("AzEl"); bprintf(startup, "Starting serial thread for motors."); /* initialize values in the motor info structs */ azinfo.fd = 0; azinfo.open = 0; azinfo.init = 0; azinfo.closing = 0; //azinfo.ref = az_enc; strncpy(azinfo.motorstr, "az", 3); elinfo.fd = 0; elinfo.open = 0; elinfo.init = 0; elinfo.closing = 0; //elinfo.ref = el_enc; strncpy(elinfo.motorstr, "el", 3); /* try to open the ports */ while (azinfo.open == 0) { open_cm(AZ_DEVICE, &azinfo); if (ser_attempts == 10) { bputs(err, "Unable to open az motor port after 10 attempts.\n"); } ser_attempts++; if (azinfo.open == 1) { bprintf(info, "Opened the az motor port on attempt %i", ser_attempts); } else sleep(1); } ser_attempts = 0; while (elinfo.open == 0) { open_cm(EL_DEVICE, &elinfo); if (ser_attempts == 10) { bputs(err, "Unable to open el motor port after 10 attempts.\n"); } ser_attempts++; if (elinfo.open == 1) { bprintf(info, "Opened the el motor port on attempt %i", ser_attempts); } else sleep(1); } /* initialize motor internal parameters */ bprintf(info, "Initializing az motor"); while (azinfo.init == 0) { init_cm(&azinfo); if (init_attempts == 10) { bputs(err, "Could not initalize the az motor after 10 attempts.\n"); } init_attempts++; if (azinfo.init == 1) { bprintf(info, "Initialized the az motor on attempt %i", init_attempts); } else sleep(1); } init_attempts = 0; bprintf(info, "Initializing el motor"); while (elinfo.init == 0) { init_cm(&elinfo); if (init_attempts == 10) { bputs(err, "Could not initalize the el motor after 10 attempts.\n"); } init_attempts++; if (elinfo.init == 1) { bprintf(info, "Initialized the el motor on attempt %i", init_attempts); } else sleep(1); } while (1) { /* TODO: NEED SOME KIND OF CHECKING OF ERROR STATES HERE */ if (azinfo.closing == 1) { close_cm(&azinfo); usleep(10000); } if (elinfo.closing == 1) { close_cm(&elinfo); usleep(10000); } else { AzElScan(); usleep(10000); /* TODO - sjb: probably want, "too many errors, reconnecting logic here */ } } return NULL; }