static int
control__activate(struct sensors_control_device_t *dev,
int handle,
int enabled)
{
D("%s", __FUNCTION__);
SensorControl* ctl = (void*)dev;
uint32_t mask, sensors, active, new_sensors, changed;
int ret;
D("%s: handle=%s (%d) enabled=%d", __FUNCTION__,
_sensorIdToName(handle), handle, enabled);
if (!ID_CHECK(handle)) {
E("%s: bad handle ID", __FUNCTION__);
return -1;
}
mask = (1<<handle);
sensors = enabled ? mask : 0;
active = active_sensors;
new_sensors = (active & ~mask) | (sensors & mask);
changed = active ^ new_sensors;
if (!changed)
return 0;
active_sensors = new_sensors;
return 0;
}
static void idFree(u_short id)
{
if (ID_CHECK(id) == 0) {
WARN("idAlloc - %04x is not in use\n", id) ;
return ;
}
ID_CLEAR(id) ;
}
static int sensor_device_activate(struct sensors_poll_device_t *dev0,
int handle,
int enabled)
{
SensorDevice* dev = (void*)dev0;
D("%s: handle=%s (%d) enabled=%d", __FUNCTION__,
_sensorIdToName(handle), handle, enabled);
/* Sanity check */
if (!ID_CHECK(handle)) {
E("%s: bad handle ID", __FUNCTION__);
return -EINVAL;
}
/* Exit early if sensor is already enabled/disabled. */
uint32_t mask = (1U << handle);
uint32_t sensors = enabled ? mask : 0;
pthread_mutex_lock(&dev->lock);
uint32_t active = dev->active_sensors;
uint32_t new_sensors = (active & ~mask) | (sensors & mask);
uint32_t changed = active ^ new_sensors;
int ret = 0;
if (changed) {
/* Send command to the emulator. */
char command[64];
snprintf(command,
sizeof command,
"set:%s:%d",
_sensorIdToName(handle),
enabled != 0);
ret = sensor_device_send_command_locked(dev, command);
if (ret < 0) {
E("%s: when sending command errno=%d: %s", __FUNCTION__, -ret,
strerror(-ret));
} else {
dev->active_sensors = new_sensors;
}
}
pthread_mutex_unlock(&dev->lock);
return ret;
}
static BOOL idAlloc(u_short *id)
{
u_short newid ;
for (newid = (msgidLast + 1) & 0xffff ;
newid != msgidLast ;
newid = (newid + 1) & 0xffff) {
if (ID_CHECK(newid) == 0) {
ID_USEIT(newid) ;
msgidLast = newid ;
*id = newid ;
return TRUE ;
}
}
WARN("idAlloc - no more ID\n") ;
return FALSE ;
}
static int
control__activate(struct sensors_poll_device_t *dev,
int handle,
int enabled)
{
SensorPoll* ctl = (void*)dev;
uint32_t mask, sensors, active, new_sensors, changed;
char command[128];
int ret;
D("%s: handle=%s (%d) fd=%d enabled=%d", __FUNCTION__,
_sensorIdToName(handle), handle, ctl->fd, enabled);
if (!ID_CHECK(handle)) {
E("%s: bad handle ID", __FUNCTION__);
return -1;
}
mask = (1<<handle);
sensors = enabled ? mask : 0;
active = ctl->active_sensors;
new_sensors = (active & ~mask) | (sensors & mask);
changed = active ^ new_sensors;
if (!changed)
return 0;
snprintf(command, sizeof command, "set:%s:%d",
_sensorIdToName(handle), enabled != 0);
if (ctl->fd < 0) {
ctl->fd = qemud_channel_open(SENSORS_SERVICE_NAME);
}
ret = qemud_channel_send(ctl->fd, command, -1);
if (ret < 0) {
E("%s: when sending command errno=%d: %s", __FUNCTION__, errno, strerror(errno));
return -1;
}
ctl->active_sensors = new_sensors;
return 0;
}
/*
* Main sensor sim routine
*/
int main(int argc, char **argv)
{
int rc;
int qfd;
char command[128];
char ip[16];
int port;
int ssock = -1; /* simulator socket */
int sensor_id = -1;
(void)bionic_signal(SIGPIPE, sighandler);
(void)bionic_signal(SIGKILL, sighandler);
(void)bionic_signal(SIGQUIT, sighandler);
qfd = qemud_channel_open(SENSORS_SERVICE_NAME);
if (qfd < 0) {
err("Can't get a connection to qemud!\n");
return EXIT_FAILURE;
}
/* list available / enabled sensors */
info("Emulator-enabled sensors:\n");
emu_sensors_list(qfd);
/* if we're passed arguments, use them to connect to
a sensorsimulator.java instance somewhere on the network */
if (argc > 2) {
strncpy(ip, argv[1], sizeof(ip));
port = atoi(argv[2]);
ssock = sensorsimulator_open_socket(ip, port);
if (ssock < 0) {
err("Error connecting to %s:%d (%s)", ip, port,
strerror(errno));
goto exit_socket_failure;
}
if (argc > 3)
sensor_id = atoi(argv[3]);
else {
/* ask the user for a sensor ID */
char line[64];
get_param("SensorID: ", line, "%d", &sensor_id);
}
if (!ID_CHECK(sensor_id)) {
err("Invalid SensorID");
goto exit_data_error;
}
if (sensorsimulator_enable_sensor(ssock, sensor_id) < 0) {
err("Error enabling SensorID=%d (%s)", sensor_id,
strerror(errno));
goto exit_data_error;
}
daemonize();
}
/* quick fix; enable orientation sensor */
qemud_channel_send(qfd, "set:orientation:1", -1);
while (!s_should_exit) {
float x, y, z;
char *sensor_name;
if (ssock > 0) {
usleep(SENSOR_UPDATE_PERIOD_MS * 1000);
if (sensorsimulator_get_params(ssock, sensor_id,
&x, &y, &z) < 0) {
err("Error receiving data from sensorsimulator"
" (%s)", strerror(errno));
goto exit_data_error;
}
} else {
if (get_parameters(&sensor_id, &x, &y, &z) < 0) {
err("Error receiving parameters from cmdline");
continue;
}
if (!ID_CHECK(sensor_id)) {
err("Invalid SensorID");
continue;
}
}
sensor_name = (char *)sensorId_to_name(sensor_id);
if (ssock < 0 && sensor_id == ID_ORIENTATION) {
/* the user just input X,Y,Z (pitch,roll,azimuth)
the qemu pipe is expecting Z,X,Y (azimuth,pitch,roll)
*/
float tmpX = x;
float tmpY = y;
x = z;
y = tmpX;
z = tmpY;
}
if (y > 180 || y < -180) {
err("pitch out of range\n");
continue;
}
if (z > 90 || z < -90) {
err("roll out of range\n");
continue;
}
if (x >= 360 || x < 0) {
err("azimuth out of range\n");
continue;
}
snprintf(command, sizeof(command), "update-sensor:%d:%g:%g:%g:",
sensor_id, x, y, z);
dbg("simulating %s event: %s\n", sensor_name, command);
rc = qemud_channel_send(qfd, command, -1);
if (rc < 0) {
err("Error sending emulator command: %s\n",
strerror(errno));
sleep(2); /* let the user know there was a problem */
}
}
{
char tbuf[32];
time_t tm = time(NULL);
info("sensorsim exiting at %s\n", ctime_r(&tm, tbuf));
}
close(ssock);
close(qfd);
return EXIT_SUCCESS;
exit_data_error:
close(ssock);
exit_socket_failure:
close(qfd);
return EXIT_FAILURE;
}
