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