static void show_data() { float result[6]; uint32_t deltat = 0; uint16_t ch3; float min[6], max[6]; uint8_t i; for (i=0; i<6; i++) { /* clearly out of bounds values: */ min[i] = 99999999.0f; max[i] = -88888888.0f; } do { ch3 = adc.Ch(3); deltat += adc.Ch6(channel_map, result); for (i=0; i<6; i++) { if (result[i] < min[i]) min[i] = result[i]; if (result[i] > max[i]) max[i] = result[i]; if (fabsf(result[i]) > 0x8000) { hal.console->printf("result[%u]=%f\n", (unsigned)i, result[i]); } } } while ((AP_HAL::millis() - timer) < 200); timer = AP_HAL::millis(); hal.console->printf("g=(%f,%f,%f) a=(%f,%f,%f) +/-(%.0f,%.0f,%.0f,%.0f,%.0f,%.0f) gt=%u dt=%u\n", result[0], result[1], result[2], result[3], result[4], result[5], (max[0]-min[0]), (max[1]-min[1]), (max[2]-min[2]), (max[3]-min[3]), (max[4]-min[4]), (max[5]-min[5]), ch3, (unsigned)(deltat/1000)); }
static void show_timing() { uint32_t mint = (uint32_t)-1, maxt = 0, totalt=0; uint32_t start_time = AP_HAL::millis(); float result[6]; uint32_t count = 0; hal.console->println("Starting timing test"); adc.Ch6(channel_map, result); do { uint32_t deltat = adc.Ch6(channel_map, result); if (deltat > maxt) maxt = deltat; if (deltat < mint) mint = deltat; totalt += deltat; count++; } while ((AP_HAL::millis() - start_time) < 5000); hal.console->printf("timing: mint=%lu maxt=%lu avg=%lu\n", mint, maxt, totalt/count); }
/* copy data from ADC to frontend */ bool AP_InertialSensor_Oilpan::update() { float adc_values[6]; apm1_adc.Ch6(_sensors, adc_values); // copy gyros to frontend Vector3f v; v(_sensor_signs[0] * ( adc_values[0] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_x, _sensor_signs[1] * ( adc_values[1] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_y, _sensor_signs[2] * ( adc_values[2] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_z); _rotate_and_correct_gyro(_gyro_instance, v); _publish_gyro(_gyro_instance, v); // copy accels to frontend v(_sensor_signs[3] * (adc_values[3] - OILPAN_RAW_ACCEL_OFFSET), _sensor_signs[4] * (adc_values[4] - OILPAN_RAW_ACCEL_OFFSET), _sensor_signs[5] * (adc_values[5] - OILPAN_RAW_ACCEL_OFFSET)); v *= OILPAN_ACCEL_SCALE_1G; _rotate_and_correct_accel(_accel_instance, v); _publish_accel(_accel_instance, v); return true; }