surbyte:
-
Buscar el error
Pregunto:
Si he interpretado correctamente tu código y esos valores son impresos por
DEBUG_PRINT(val[3] * M_PI/180);
DEBUG_PRINT(val[4] * M_PI/180);
DEBUG_PRINT(val[5] * M_PI/180);
DEBUG_PRINT(val[0]);
DEBUG_PRINT(val[1]);
DEBUG_PRINT(val[2]);
DEBUG_PRINT(val[6]);
DEBUG_PRINT(val[7]);
DEBUG_PRINT(val[8]);
y los valores de val son lecturas de los sensores, entonces como puede emitir valores corruptos?
Ahora no está disponible "DebugUtils.h" que creo contiene DEBUG_PRINT(value)
Confirma si estoy en lo correcto o no.
Gracias surbyte por contestar.
Las líneas que indicas están comentadas. Las lecturas de los acel y gyro se obtienen con getValues de la siguiente manera(aquí, al ser la librería, está el genérico para todos los tipos de sensores; lleven Magnetómetro o no, S. de Presión o no…):
void FreeIMU::getValues(float * values) {
float acgyro_corr[9] = {0.,0.,0.,0.,0.,0.,0.,0.,0.};
float values_cal[9] = {0.,0.,0.,0.,0.,0.,0.,0.,0.};
uint8_t i;
#if HAS_ITG3200() //assumes adxl3345
int accval[3];
acc.readAccel(&accval[0], &accval[1], &accval[2]);
accval[0] = mfilter_accx.filter((float) accval[0]);
accval[1] = mfilter_accy.filter((float) accval[1]);
accval[2] = mfilter_accz.filter((float) accval[2]);
gyro.readGyro(&values_cal[3]);
gyro.readTemp(&senTemp);
if(temp_corr_on == 1) {
if(senTemp < senTemp_break) {
for(i = 0; i < 9; i++) {
acgyro_corr[i] = c3[i]*(senTemp*senTemp*senTemp) + c2[i]*(senTemp*senTemp) + c1[i]*senTemp + c0[i];
}
}
} else {
for(i = 0; i < 9; i++) {
acgyro_corr[i] = 0.0f;
}
}
values_cal[0] = (float) accval[0] - acgyro_corr[0];
values_cal[1] = (float) accval[1] - acgyro_corr[1];
values_cal[2] = (float) accval[2] - acgyro_corr[2];
values_cal[3] = (values_cal[3] - gyro_off_x)/gyro_sensitivity;
values_cal[4] = (values_cal[4] - gyro_off_y)/gyro_sensitivity;
values_cal[5] = (values_cal[5] - gyro_off_z)/gyro_sensitivity;
#elif HAS_ALTIMU10()
gyro.read();
compass.read();
values_cal[0] = (float) compass.a.x;
values_cal[1] = (float) compass.a.y;
values_cal[2] = (float) compass.a.z;
//values[0] = mfilter_accx.filter((float) compass.a.x);
//values[1] = mfilter_accy.filter((float) compass.a.y);
//values[2] = mfilter_accz.filter((float) compass.a.z);
values_cal[3] = (float) gyro.g.x;
values_cal[4] = (float) gyro.g.y;
values_cal[5] = (float) gyro.g.z;
values_cal[6] = (float) compass.m.x;
values_cal[7] = (float) compass.m.y;
values_cal[8] = (float) compass.m.z;
values_cal[3] = (values_cal[3] - gyro_off_x) / gyro_sensitivity; //Sensitivity set at 70 for +/-2000 deg/sec, L3GD20H
values_cal[4] = (values_cal[4] - gyro_off_y) / gyro_sensitivity;
values_cal[5] = (values_cal[5] - gyro_off_z) / gyro_sensitivity;
#else // MPU6050
int16_t accgyroval[9];
#if HAS_MPU9150() || HAS_MPU9250()
mag.getHeading(&accgyroval[6], &accgyroval[7], &accgyroval[8]);
delay(10);
accgyro.getMotion6(&accgyroval[0], &accgyroval[1], &accgyroval[2],
&accgyroval[3], &accgyroval[4], &accgyroval[5]);
// read raw heading measurements from device
accgyroval[0] = mfilter_accx.filter((float) accgyroval[0]);
accgyroval[1] = mfilter_accy.filter((float) accgyroval[1]);
accgyroval[2] = mfilter_accz.filter((float) accgyroval[2]);
values_cal[6] = mfilter_mx.filter((float) accgyroval[6]);
values_cal[7] = mfilter_my.filter((float) accgyroval[7]);
values_cal[8] = mfilter_mz.filter((float) accgyroval[8]);
#else
accgyro.getMotion6(&accgyroval[0], &accgyroval[1], &accgyroval[2],
&accgyroval[3], &accgyroval[4], &accgyroval[5]);
#endif
DTemp = accgyro.getTemperature();
if(temp_corr_on == 1){
if(DTemp < temp_break){
for( i = 0; i < 9; i++) {
acgyro_corr[i] = c3[i]*(DTemp*DTemp*DTemp) + c2[i]*(DTemp*DTemp) + c1[i]*DTemp + c0[i];
}
}
} else {
for( i = 0; i < 9; i++) {
acgyro_corr[i] = 0.0f;
}
}
// remove offsets from the gyroscope
if(temp_corr_on == 1){
//accgyroval[3] = accgyroval[3] - acgyro_corr[3];
//accgyroval[4] = accgyroval[4] - acgyro_corr[4];
//accgyroval[5] = accgyroval[5] - acgyro_corr[5];
values_cal[3] = (float) accgyroval[3] - acgyro_corr[3];
values_cal[4] = (float) accgyroval[4] - acgyro_corr[4];
values_cal[5] = (float) accgyroval[5] - acgyro_corr[5];
}
else {
values_cal[3] = (float) accgyroval[3] - gyro_off_x;
values_cal[4] = (float) accgyroval[4] - gyro_off_y;
values_cal[5] = (float) accgyroval[5] - gyro_off_z;
}
for( i = 0; i<6; i++) {
if( i < 3 ) {
values_cal[i] = (float) accgyroval[i] - acgyro_corr[i];
}
else {
//values[i] = ((float) accgyroval[i] - acgyro_corr[i])/ 16.4f; // NOTE: this depends on the sensitivity chosen
values_cal[i] = values_cal[i] / gyro_sensitivity; //for 6050 etc
}
}
#endif
#warning Accelerometer calibration active: have you calibrated your device?
// remove offsets and scale accelerometer (calibration)
values_cal[0] = (values_cal[0] - acc_off_x) / acc_scale_x;
values_cal[1] = (values_cal[1] - acc_off_y) / acc_scale_y;
values_cal[2] = (values_cal[2] - acc_off_z) / acc_scale_z;
#if HAS_HMC5883L()
magn.getValues(&values_cal[6]);
#endif
#if HAS_HMC5883L() || HAS_MPU9150() || HAS_MPU9250() || HAS_LSM303()
// calibration
if(temp_corr_on == 1) {
values_cal[6] = (values_cal[6] - acgyro_corr[6] - magn_off_x) / magn_scale_x;
values_cal[7] = (values_cal[7] - acgyro_corr[7] - magn_off_y) / magn_scale_y;
values_cal[8] = (values_cal[8] - acgyro_corr[8] - magn_off_z) / magn_scale_z;
}
else {
#warning Magnetometer calibration active: have you calibrated your device?
values_cal[6] = (values_cal[6] - magn_off_x) / magn_scale_x;
values_cal[7] = (values_cal[7] - magn_off_y) / magn_scale_y;
values_cal[8] = (values_cal[8] - magn_off_z) / magn_scale_z;
}
#endif
for(int i = 0; i < 9; i++) {
values[i] = sensor_sign[i] * values_cal[sensor_order[i]];
}
}
En cuanto a lo que decías del fallo del sensor, no es posible debido a que si transmites vía cable USB el mensaje llega intacto y cada línea de la comunicación serie con el mismo número de caracteres ASCII
Me pongo con el cálculo del CRC que, ahora que me lo menciona, algo me suena haber leído.
Muchas, muchas gracias. Cuando obtenga algún resultado, ya sea satisfactorio o no lo posteo
Saludos
