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