Beispiel #1
0
int threads_linux_init(void)
{
    int status = 0;
    int n = 0;
    int return_value = THREADS_LINUX_SUCCESS;

    // Init data
    status = sensors_init(&battery_data, &gps_data, &imu_data, &pitot_data, &pwm_read_data, &pwm_write_data, &scp1000_data, &sonar_data);
    if(status != SENSORS_SUCCESS)
    {
        return_value = THREADS_LINUX_FAILURE;
    }

    if(return_value != THREADS_LINUX_FAILURE)
    {
        status = calibration_init(&calibration_local_coordinate_system_data, &calibration_local_fields_data, &calibration_altimeter_data);
        if(status != CALIBRATION_SUCCESS)
        {
            return_value = THREADS_LINUX_FAILURE;
        }
    }

    if(return_value != THREADS_LINUX_FAILURE)
    {
        status = gps_init(&gps_measure);
        if(status != 1)
        {
            return_value = THREADS_LINUX_FAILURE;
        }
    }

    if(return_value != THREADS_LINUX_FAILURE)
    {
        status = imu_init(&imu_measure);
        if(status != 1)
        {
            return_value = THREADS_LINUX_FAILURE;
        }
    }

    if(return_value != THREADS_LINUX_FAILURE)
    {
        status = magnetometer_init(&magnetometer_measure);
        if(status != 1)
        {
            return_value = THREADS_LINUX_FAILURE;
        }
    }

    if(return_value != THREADS_LINUX_FAILURE)
    {
    	status = sonar_init(&sonar_measure);
    	if(status != 1)
    	{
    		return_value = THREADS_LINUX_FAILURE;
    	}
    }

    if(return_value != THREADS_LINUX_FAILURE)
    {
        status = estimation_init(ESTIMATION_DO_NOT_ESTIMATE_ACCEL_BIAS, &estimation_data);
        if(status != ESTIMATION_SUCCESS)
        {
            return_value = THREADS_LINUX_FAILURE;
        }
    }

    if(return_value != THREADS_LINUX_FAILURE)
    {
        status = control_init(&control_data);
        if(status != CONTROL_SUCCESS)
        {
            return_value = THREADS_LINUX_FAILURE;
        }
    }

    if(return_value != THREADS_LINUX_FAILURE)
    {
        status = protocol_init();
        if(status != PROTOCOL_SUCCESS)
        {
            return_value = THREADS_LINUX_FAILURE;
        }
    }

    if(return_value != THREADS_LINUX_FAILURE)
    {
        status = ui_init();
        if(status != UI_SUCCESS)
        {
            return_value = THREADS_LINUX_FAILURE;
        }
    }

    if(return_value != THREADS_LINUX_FAILURE)
    {
        status = datalogger_init();
        if(status != DATALOGGER_SUCCESS)
        {
            return_value = THREADS_LINUX_FAILURE;
        }
    }

    // Main Loop
    if(return_value != THREADS_LINUX_FAILURE)
    {
        threads_linux_timer_start_task_1();
        usleep(0.5*task_1_period_us);
        threads_linux_timer_start_task_2();
        usleep(5*task_1_period_us);

        while(quittask == 0)
        {
            #if ANU_COMPILE_FOR_OVERO
            #else
            if(datalogger_status() == DATALOGGER_RUNNING) datalogger_update_IPC();
            #endif
            if(++n>100)
            {
                if(datalogger_status() == DATALOGGER_RUNNING) datalogger_write_file();
                n = 0;
            }
            usleep(5*task_1_period_us);
        }

        threads_linux_timer_stop_task_1();
        usleep(TASK1_PERIOD_US);
        threads_linux_timer_stop_task_2();
        usleep(TASK2_PERIOD_US);
    }

    status = datalogger_close();
    if(status != DATALOGGER_SUCCESS)
    {
        return_value = THREADS_LINUX_FAILURE;
    }

    status = ui_close();
    if(status != UI_SUCCESS)
    {
        return_value = THREADS_LINUX_FAILURE;
    }

    status = protocol_close();
    if(status != PROTOCOL_SUCCESS)
    {
        return_value = THREADS_LINUX_FAILURE;
    }

    status = control_close();
    if(status != CONTROL_SUCCESS)
    {
        return_value = THREADS_LINUX_FAILURE;
    }

    status = estimation_close(&estimation_data);
    if(status != ESTIMATION_SUCCESS)
    {
        return_value = THREADS_LINUX_FAILURE;
    }

    status = magnetometer_close(&magnetometer_measure);
    if(status != 1)
    {
        return_value = THREADS_LINUX_FAILURE;
    }

    status = imu_close(&imu_measure);
    if(status != 1)
    {
        return_value = THREADS_LINUX_FAILURE;
    }
    status = gps_close(&gps_measure);
    if(status != 1)
    {
        return_value = THREADS_LINUX_FAILURE;
    }

    status = calibration_close(&calibration_local_coordinate_system_data, &calibration_local_fields_data, &calibration_altimeter_data);
    if(status != CALIBRATION_SUCCESS)
    {
        return_value = THREADS_LINUX_FAILURE;
    }

    status = sensors_close();
    if(status != SENSORS_SUCCESS)
    {
        return_value = THREADS_LINUX_FAILURE;
    }

    return return_value;
}
int main(int argc, char** argv)
{
    int err;
    struct sensors_poll_device_t* device;
    struct sensors_module_t* module;

    err = hw_get_module(SENSORS_HARDWARE_MODULE_ID, (hw_module_t const**)&module);
    if (err != 0) {
        printf("hw_get_module() failed (%s)\n", err);
        return 0;
    }

    err = sensors_open(&module->common, &device);
    if (err != 0) {
        printf("sensors_open() failed (%s)\n", err);
        return 0;
    }

    printf ("Opened sensors\n");

    struct sensor_t const* list;
    int count = module->get_sensors_list(module, &list);
    printf("%d sensors found:\n", count);
    int i =0;
    for (i ; i<count ; i++) {
#if 0
        printf("%s\n"
                "\tvendor: %s\n"
                "\tversion: %d\n"
                "\thandle: %d\n"
                "\ttype: %d\n"
                "\tmaxRange: %f\n"
                "\tresolution: %f\n"
                "\tpower: %f mA\n",
                list[i].name,
                list[i].vendor,
                list[i].version,
                list[i].handle,
                list[i].type,
                list[i].maxRange,
                list[i].resolution,
                list[i].power);
#endif

        printf("%s\n"
                "\tvendor: %s\n"
                "\tversion: %d\n"
                "\thandle: %d\n"
               "\ttype: %d\n",
                list[i].name,
                list[i].vendor,
                list[i].version,
                list[i].handle,
                list[i].type);


    }

    static const size_t numEvents = 16;
    sensors_event_t buffer[numEvents];
    i = 0;
    for (i ; i<count ; i++) {
      printf("De-activate \n");
        err = device->activate(device, list[i].handle, 0);
        if (err != 0) {
            printf("deactivate() for '%s'failed (%s)\n",
                    list[i].name, err);
            return 0;
        }
    }
    i = 0;
    for (i ; i<count ; i++) {
        err = device->activate(device, list[i].handle, 1);
        if (err != 0) {
            printf("activate() for '%s'failed (%s)\n",
                    list[i].name, err);
            return 0;
        }
        device->setDelay(device, list[i].handle, 1);
    }

    do {
        int n = device->poll(device, buffer, numEvents);
        if (n < 0) {
            printf("poll() failed (%s)\n", err);
            break;
        }
        i = 0;
        //printf("read %d events:\n", n);
        for ( i ; i<n ; i++) {
            const sensors_event_t data = buffer[i];

            if (data.version != sizeof(sensors_event_t)) {
                printf("incorrect event version (version=%d, expected=%d",
                        data.version, sizeof(sensors_event_t));
                break;
            }

            switch(data.type) {
                case SENSOR_TYPE_ACCELEROMETER:
                case SENSOR_TYPE_MAGNETIC_FIELD:
                case SENSOR_TYPE_ORIENTATION:
                case SENSOR_TYPE_GYROSCOPE:
                case SENSOR_TYPE_GRAVITY:
                case SENSOR_TYPE_LINEAR_ACCELERATION:
                case SENSOR_TYPE_ROTATION_VECTOR:
                    printf("sensor=%s, time=%lld, value=<%5.1f,%5.1f,%5.1f>\n",
                            getSensorName(data.type),
                            data.timestamp,
                            data.data[0],
                            data.data[1],
                            data.data[2]);
                    break;

                case SENSOR_TYPE_LIGHT:
                case SENSOR_TYPE_PRESSURE:
                case SENSOR_TYPE_TEMPERATURE:
                case SENSOR_TYPE_PROXIMITY:
                case SENSOR_TYPE_RELATIVE_HUMIDITY:
                case SENSOR_TYPE_AMBIENT_TEMPERATURE:
                  /*printf("sensor=%s, time=%lld, value=%f\n",
                            getSensorName(data.type),
                            data.timestamp,
                            data.data[0]);
                  */
                    break;

                default:
                    printf("sensor=%d, time=%lld, value=<%f,%f,%f, ...>\n",
                            data.type,
                            data.timestamp,
                            data.data[0],
                            data.data[1],
                            data.data[2]);
                    break;
            }
        }
    } while (1); // fix that

    i = 0;
    for (i ; i<count ; i++) {
        err = device->activate(device, list[i].handle, 0);
        if (err != 0) {
            printf("deactivate() for '%s'failed (%s)\n",
                    list[i].name, err);
            return 0;
        }
    }

    err = sensors_close(device);
    if (err != 0) {
        printf("sensors_close() failed (%s)\n", err);
    }
    return 0;
}