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