sensors_trout.c

Android平台上Midware层源代码合集

        }
    }
#endif
}

static uint32_t read_sensors_state(int fd)
{
    if (fd<0) return 0;
    short flags;
    uint32_t sensors = 0;
    // read the actual value of all sensors
    if (!ioctl(fd, ECS_IOCTL_APP_GET_MFLAG, &flags)) {
        if (flags)  sensors |= SENSORS_ORIENTATION;
        else        sensors &= ~SENSORS_ORIENTATION;
    }
    if (!ioctl(fd, ECS_IOCTL_APP_GET_AFLAG, &flags)) {
        if (flags)  sensors |= SENSORS_ACCELERATION;
        else        sensors &= ~SENSORS_ACCELERATION;
    }
    if (!ioctl(fd, ECS_IOCTL_APP_GET_TFLAG, &flags)) {
        if (flags)  sensors |= SENSORS_TEMPERATURE;
        else        sensors &= ~SENSORS_TEMPERATURE;
    }
#ifdef ECS_IOCTL_APP_SET_MVFLAG
    if (!ioctl(fd, ECS_IOCTL_APP_GET_MVFLAG, &flags)) {
        if (flags)  sensors |= SENSORS_MAGNETIC_FIELD;
        else        sensors &= ~SENSORS_MAGNETIC_FIELD;
    }
#endif
    return sensors;
}

/*****************************************************************************/

uint32_t sensors_control_init()
{
    return SUPPORTED_SENSORS;
}

int sensors_control_open()
{
    return open_input();
}

uint32_t sensors_control_activate(uint32_t sensors, uint32_t mask)
{
    mask &= SUPPORTED_SENSORS;
    uint32_t active = sActiveSensors;
    uint32_t new_sensors = (active & ~mask) | (sensors & mask);
    uint32_t changed = active ^ new_sensors;
    if (changed) {
        int fd = open_akm();
        if (fd < 0) return 0;

        if (!active && new_sensors) {
            // force all sensors to be updated
            changed = SUPPORTED_SENSORS;
        }

        enable_disable(fd, new_sensors, changed);

        if (active && !new_sensors) {
            // close the driver
            close_akm();
        }
        sActiveSensors = active = new_sensors;
        LOGD("sensors=%08x, real=%08x",
                sActiveSensors, read_sensors_state(fd));
    }
    return active;
}

int sensors_control_delay(int32_t ms)
{
#ifdef ECS_IOCTL_APP_SET_DELAY
    if (sAkmFD <= 0) {
        return -1;
    }
    short delay = ms;
    if (!ioctl(sAkmFD, ECS_IOCTL_APP_SET_DELAY, &delay)) {
        return -errno;
    }
    return 0;
#else
    return -1;
#endif
}

/*****************************************************************************/

#define MAX_NUM_SENSORS 8
static int sInputFD = -1;
static const int ID_O  = 0;
static const int ID_A  = 1;
static const int ID_T  = 2;
static const int ID_M  = 3;
static const int ID_OR = 7; // orientation raw
static sensors_data_t sSensors[MAX_NUM_SENSORS];
static uint32_t sPendingSensors;

int sensors_data_open(int fd)
{
    int i;
    LMSInit();
    memset(&sSensors, 0, sizeof(sSensors));
    for (i=0 ; i<MAX_NUM_SENSORS ; i++) {
        // by default all sensors have high accuracy
        // (we do this because we don't get an update if the value doesn't
        // change).
        sSensors[i].vector.status = SENSOR_STATUS_ACCURACY_HIGH;
    }
    sPendingSensors = 0;
    sInputFD = dup(fd);
    LOGD("sensors_data_open: fd = %d", sInputFD);
    return 0;
}

int sensors_data_close()
{
    close(sInputFD);
    sInputFD = -1;
    return 0;
}

static int pick_sensor(sensors_data_t* values)
{
    uint32_t mask = SENSORS_MASK;
    while(mask) {
        uint32_t i = 31 - __builtin_clz(mask);
        mask &= ~(1<<i);
        if (sPendingSensors & (1<<i)) {
            sPendingSensors &= ~(1<<i);
            *values = sSensors[i];
            values->sensor = (1<<i);
            LOGD_IF(0, "%d [%f, %f, %f]", (1<<i),
                    values->vector.x,
                    values->vector.y,
                    values->vector.z);
            return (1<<i);
        }
    }
    LOGE("No sensor to return!!! sPendingSensors=%08x", sPendingSensors);
    // we may end-up in a busy loop, slow things down, just in case.
    usleep(100000);
    return -1;
}

int sensors_data_poll(sensors_data_t* values, uint32_t sensors_of_interest)
{
    struct input_event event;
    int nread;
    int64_t t;

    int fd = sInputFD;
    if (fd <= 0)
        return -1;

    // there are pending sensors, returns them now...
    if (sPendingSensors) {
        return pick_sensor(values);
    }

    uint32_t new_sensors = 0;
    struct pollfd fds;
    fds.fd = fd;
    fds.events = POLLIN;
    fds.revents = 0;

    // wait until we get a complete event for an enabled sensor
    while (1) {
        nread = 0;
        if (sensors_of_interest & SENSORS_ORIENTATION) {
            /* We do some special processing if the orientation sensor is
             * activated. In particular the yaw value is filtered with a
             * LMS filter. Since the kernel only sends an event when the
             * value changes, we need to wake up at regular intervals to
             * generate an output value (the output value may not be
             * constant when the input value is constant)
             */
            int err = poll(&fds, 1, SENSORS_TIMEOUT_MS);
            if (err == 0) {
                struct timespec time;
                time.tv_sec = time.tv_nsec = 0;
                clock_gettime(CLOCK_MONOTONIC, &time);

                /* generate an output value */
                t = time.tv_sec*1000000000LL+time.tv_nsec;
                new_sensors |= SENSORS_ORIENTATION;
                sSensors[ID_O].orientation.yaw =
                        LMSFilter(t, sSensors[ID_O].orientation.yaw);

                /* generate a fake sensors event */
                event.type = EV_SYN;
                event.time.tv_sec = time.tv_sec;
                event.time.tv_usec = time.tv_nsec/1000;
                nread = sizeof(event);
            }
        }
        if (nread == 0) {
            /* read the next event */
            nread = read(fd, &event, sizeof(event));
        }
        if (nread == sizeof(event)) {
            uint32_t v;
            if (event.type == EV_ABS) {
                //LOGD("type: %d code: %d value: %-5d time: %ds",
                //        event.type, event.code, event.value,
                //      (int)event.time.tv_sec);
                switch (event.code) {

                    case EVENT_TYPE_ACCEL_X:
                        new_sensors |= SENSORS_ACCELERATION;
                        sSensors[ID_A].acceleration.x = event.value * CONVERT_A_X;
                        break;
                    case EVENT_TYPE_ACCEL_Y:
                        new_sensors |= SENSORS_ACCELERATION;
                        sSensors[ID_A].acceleration.y = event.value * CONVERT_A_Y;
                        break;
                    case EVENT_TYPE_ACCEL_Z:
                        new_sensors |= SENSORS_ACCELERATION;
                        sSensors[ID_A].acceleration.z = event.value * CONVERT_A_Z;
                        break;

                    case EVENT_TYPE_MAGV_X:
                        new_sensors |= SENSORS_MAGNETIC_FIELD;
                        sSensors[ID_M].magnetic.x = event.value * CONVERT_M_X;
                        break;
                    case EVENT_TYPE_MAGV_Y:
                        new_sensors |= SENSORS_MAGNETIC_FIELD;
                        sSensors[ID_M].magnetic.y = event.value * CONVERT_M_Y;
                        break;
                    case EVENT_TYPE_MAGV_Z:
                        new_sensors |= SENSORS_MAGNETIC_FIELD;
                        sSensors[ID_M].magnetic.z = event.value * CONVERT_M_Z;
                        break;

                    case EVENT_TYPE_YAW:
                        new_sensors |= SENSORS_ORIENTATION | SENSORS_ORIENTATION_RAW;
                        t = event.time.tv_sec*1000000000LL +
                                event.time.tv_usec*1000;
                        sSensors[ID_O].orientation.yaw = 
                            (sensors_of_interest & SENSORS_ORIENTATION) ?
                                    LMSFilter(t, event.value) : event.value;
                        sSensors[ID_OR].orientation.yaw = event.value;
                        break;
                    case EVENT_TYPE_PITCH:
                        new_sensors |= SENSORS_ORIENTATION | SENSORS_ORIENTATION_RAW;
                        sSensors[ID_O].orientation.pitch = event.value;
                        sSensors[ID_OR].orientation.pitch = event.value;
                        break;
                    case EVENT_TYPE_ROLL:
                        new_sensors |= SENSORS_ORIENTATION | SENSORS_ORIENTATION_RAW;
                        sSensors[ID_O].orientation.roll = event.value;
                        sSensors[ID_OR].orientation.roll = event.value;
                        break;

                    case EVENT_TYPE_TEMPERATURE:
                        new_sensors |= SENSORS_TEMPERATURE;
                        sSensors[ID_T].temperature = event.value;
                        break;

                    case EVENT_TYPE_STEP_COUNT:
                        // step count (only reported in MODE_FFD)
                        // we do nothing with it for now.
                        break;
                    case EVENT_TYPE_ACCEL_STATUS:
                        // accuracy of the calibration (never returned!)
                        //LOGD("G-Sensor status %d", event.value);
                        break;
                    case EVENT_TYPE_ORIENT_STATUS:
                        // accuracy of the calibration
                        v = (uint32_t)(event.value & SENSOR_STATE_MASK);
                        LOGD_IF(sSensors[ID_O].orientation.status != (uint8_t)v,
                                "M-Sensor status %d", v);
                        sSensors[ID_O].orientation.status = (uint8_t)v;
                        sSensors[ID_OR].orientation.status = (uint8_t)v;
                        break;
                }
            } else if (event.type == EV_SYN) {
                if (new_sensors) {
                    sPendingSensors = new_sensors;
                    int64_t t = event.time.tv_sec*1000000000LL +
                            event.time.tv_usec*1000;
                    while (new_sensors) {
                        uint32_t i = 31 - __builtin_clz(new_sensors);
                        new_sensors &= ~(1<<i);
                        sSensors[i].time = t;
                    }
                    return pick_sensor(values);
                }
            }
        }
    }
}

uint32_t sensors_data_get_sensors() {
    return SUPPORTED_SENSORS;
}