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;
}
