i am developing a quadcopter using FreeRTOS and Ardupilot 2.5 . I have a problem transforming the gyro and reading data from it. This is the code i have writen but the gyro output is not the one expected:
if(xQueueReceive(xQueueIMUData,&imu_data,RECEIVE_DELAY))
{
//new_time = millis();
//new_time = new_time - time_past;
new_time1 = xTaskGetTickCount();
/Serial.printf("new_time %u\t",new_time1);/
new_time = float(new_time1 - time_past);
new_time = new_time / 1000;
fXg=imu_data[ACC_X]0.05+fXg0.95;
fZg=imu_data[ACC_Z]0.05+fZg0.95;
fYg=imu_data[ACC_Y]0.05+fYg0.95;
out_data[ACC_roll] = (atan2(fYg , -fZg))*RADIANI_IN_GRADE;
out_data[ACC_pitch] = (atan2(fXg ,-fZg))*RADIANI_IN_GRADE; // -90 este offset setat manual.
temp_out_data[Gt1] = imu_data[GYRO_X]*0.01;
temp_out_data[Gt2] = imu_data[GYRO_Y]*new_time;
temp_out_data[Gt3] = imu_data[GYRO_Z]*new_time;
out_data[G_roll] += temp_out_data[Gt1]; //unghiul din urma
out_data[G_pitch] += temp_out_data[Gt2];
out_data[G_yaw] += temp_out_data[Gt3];
/if(out_data[ACC_pitch]<=180) out_data[ACC_pitch]=-1.0out_data[ACC_pitch];
if(out_data[ACC_pitch]>180) out_data[ACC_pitch]=360-out_data[ACC_pitch];*/
time_past = xTaskGetTickCount();
//Serial.printf("Diferenta de timp %4.3f\t",new_time);
new_time = 0;
gyro=toD
for (int i = G_roll; i<=G_roll; i++)
{
Serial.printf("out_data[%d]= %4.5f\r\n",i,out_data*);*
and the output from 0 to 90 degrees :
out_data[2]= 0.04592
out_data[2]= 0.06777
out_data[2]= 0.09385
out_data[2]= 0.12149
out_data[2]= 0.14772
out_data[2]= 0.17068
out_data[2]= 0.18946
out_data[2]= 0.20430
out_data[2]= 0.21612
out_data[2]= 0.22516
out_data[2]= 0.23177
out_data[2]= 0.23699
out_data[2]= 0.24247
out_data[2]= 0.24999
out_data[2]= 0.26056
out_data[2]= 0.27442
out_data[2]= 0.29086
out_data[2]= 0.30839
out_data[2]= 0.32539
out_data[2]= 0.34072
out_data[2]= 0.35378
out_data[2]= 0.36436
out_data[2]= 0.37232
out_data[2]= 0.37809
out_data[2]= 0.38256
out_data[2]= 0.38680
out_data[2]= 0.39182
out_data[2]= 0.39843
out_data[2]= 0.40727
out_data[2]= 0.41897
out_data[2]= 0.43357
out_data[2]= 0.44981
out_data[2]= 0.46650
out_data[2]= 0.48257
out_data[2]= 0.49715
out_data[2]= 0.51011
out_data[2]= 0.52190
out_data[2]= 0.53255
out_data[2]= 0.54293
out_data[2]= 0.55371
out_data[2]= 0.56500
out_data[2]= 0.57692
out_data[2]= 0.58948
out_data[2]= 0.60187
out_data[2]= 0.61407
out_data[2]= 0.62654
out_data[2]= 0.63844
out_data[2]= 0.64970
out_data[2]= 0.66022
out_data[2]= 0.67010
out_data[2]= 0.67902
out_data[2]= 0.68709
out_data[2]= 0.69523
out_data[2]= 0.70426
out_data[2]= 0.71367
out_data[2]= 0.72359
out_data[2]= 0.73479
out_data[2]= 0.74663
out_data[2]= 0.75831
out_data[2]= 0.76985
out_data[2]= 0.78115
out_data[2]= 0.79273
out_data[2]= 0.80428
out_data[2]= 0.81534
out_data[2]= 0.82583
out_data[2]= 0.83585
out_data[2]= 0.84528
out_data[2]= 0.85437
out_data[2]= 0.86366
out_data[2]= 0.87344
out_data[2]= 0.88350
out_data[2]= 0.89395
out_data[2]= 0.90447
out_data[2]= 0.91464
out_data[2]= 0.92502
out_data[2]= 0.93620
out_data[2]= 0.94819
out_data[2]= 0.96072
out_data[2]= 0.97311
out_data[2]= 0.98486
out_data[2]= 0.99608
out_data[2]= 1.00698
out_data[2]= 1.01769
out_data[2]= 1.02848
out_data[2]= 1.03897
out_data[2]= 1.04913
out_data[2]= 1.05894
out_data[2]= 1.06827
out_data[2]= 1.07748
out_data[2]= 1.08726
out_data[2]= 1.09779
out_data[2]= 1.10869
out_data[2]= 1.12010
out_data[2]= 1.13473
out_data[2]= 1.15069
out_data[2]= 1.16359
out_data[2]= 1.17468
out_data[2]= 1.18504
out_data[2]= 1.19414
out_data[2]= 1.20236
out_data[2]= 1.21019
out_data[2]= 1.21739
out_data[2]= 1.22411
out_data[2]= 1.22997
out_data[2]= 1.23532
out_data[2]= 1.24090
out_data[2]= 1.24673
out_data[2]= 1.25590
out_data[2]= 1.26688
out_data[2]= 1.27579
out_data[2]= 1.28321
out_data[2]= 1.29112
out_data[2]= 1.29956
out_data[2]= 1.30809
out_data[2]= 1.31650
out_data[2]= 1.32418
out_data[2]= 1.33126
out_data[2]= 1.33769
out_data[2]= 1.34354
out_data[2]= 1.34854
out_data[2]= 1.35300
out_data[2]= 1.35785
out_data[2]= 1.36394
out_data[2]= 1.37136
out_data[2]= 1.37956
out_data[2]= 1.38784
out_data[2]= 1.39589
out_data[2]= 1.40326
out_data[2]= 1.40928
out_data[2]= 1.41417
out_data[2]= 1.41877
out_data[2]= 1.42278
out_data[2]= 1.42617
out_data[2]= 1.42904
out_data[2]= 1.43086
out_data[2]= 1.43149
out_data[2]= 1.43102
out_data[2]= 1.42902
out_data[2]= 1.42539
out_data[2]= 1.42133
out_data[2]= 1.41699
out_data[2]= 1.41412
out_data[2]= 1.41358
out_data[2]= 1.41578
out_data[2]= 1.41948
out_data[2]= 1.42386
out_data[2]= 1.42821
out_data[2]= 1.43216
out_data[2]= 1.43523
out_data[2]= 1.43741
out_data[2]= 1.43908
out_data[2]= 1.44012
out_data[2]= 1.44053
out_data[2]= 1.44126
out_data[2]= 1.44275
out_data[2]= 1.44485
out_data[2]= 1.44691
out_data[2]= 1.44837
out_data[2]= 1.44975
out_data[2]= 1.45117
out_data[2]= 1.45233
out_data[2]= 1.45298
out_data[2]= 1.45331
out_data[2]= 1.45355
out_data[2]= 1.45430
out_data[2]= 1.45579
out_data[2]= 1.45756
out_data[2]= 1.45903
out_data[2]= 1.45975
out_data[2]= 1.45995
out_data[2]= 1.46057
out_data[2]= 1.46212
out_data[2]= 1.46479
out_data[2]= 1.46837
out_data[2]= 1.47207
out_data[2]= 1.47530
out_data[2]= 1.47777
out_data[2]= 1.48004
out_data[2]= 1.48208
out_data[2]= 1.48374
out_data[2]= 1.48576
out_data[2]= 1.48821
out_data[2]= 1.49133
out_data[2]= 1.49344
out_data[2]= 1.49511
out_data[2]= 1.49761
out_data[2]= 1.50203
out_data[2]= 1.50772
out_data[2]= 1.51339
out_data[2]= 1.51777
out_data[2]= 1.51953
out_data[2]= 1.51841
out_data[2]= 1.51499
out_data[2]= 1.51022
out_data[2]= 1.50608
out_data[2]= 1.50383
out_data[2]= 1.50318
out_data[2]= 1.50388
out_data[2]= 1.50537
out_data[2]= 1.50635
out_data[2]= 1.50615
out_data[2]= 1.50544
out_data[2]= 1.50500
out_data[2]= 1.50536
out_data[2]= 1.50648
out_data[2]= 1.50862
out_data[2]= 1.51177
out_data[2]= 1.51510
out_data[2]= 1.51851
out_data[2]= 1.52114
out_data[2]= 1.52243
out_data[2]= 1.52298
out_data[2]= 1.52326
out_data[2]= 1.52352
out_data[2]= 1.52347
out_data[2]= 1.52314
out_data[2]= 1.52271
out_data[2]= 1.52203
out_data[2]= 1.52121
out_data[2]= 1.52097
out_data[2]= 1.52130
out_data[2]= 1.52154
out_data[2]= 1.52168
out_data[2]= 1.52145
out_data[2]= 1.52076
out_data[2]= 1.51996
out_data[2]= 1.51900
out_data[2]= 1.51764
out_data[2]= 1.51596
out_data[2]= 1.51447
out_data[2]= 1.51349
out_data[2]= 1.51295
out_data[2]= 1.51269
out_data[2]= 1.51285
out_data[2]= 1.51312
out_data[2]= 1.51328
out_data[2]= 1.51396
out_data[2]= 1.51505
out_data[2]= 1.51623
out_data[2]= 1.51699
out_data[2]= 1.51699
out_data[2]= 1.51616
out_data[2]= 1.51469
out_data[2]= 1.51309
out_data[2]= 1.51113
out_data[2]= 1.50888
out_data[2]= 1.50718
out_data[2]= 1.50669
out_data[2]= 1.50702
out_data[2]= 1.50762
out_data[2]= 1.50823
out_data[2]= 1.50893
out_data[2]= 1.50966
out_data[2]= 1.51012
out_data[2]= 1.50992
out_data[2]= 1.50905
out_data[2]= 1.50786
out_data[2]= 1.50692
out_data[2]= 1.50643
out_data[2]= 1.50630
out_data[2]= 1.50647
out_data[2]= 1.50674
out_data[2]= 1.50680
out_data[2]= 1.50650
out_data[2]= 1.50600
out_data[2]= 1.50543
out_data[2]= 1.50468
out_data[2]= 1.50366
out_data[2]= 1.50242
out_data[2]= 1.50154
out_data[2]= 1.50161
out_data[2]= 1.50246
out_data[2]= 1.50352
out_data[2]= 1.50420
out_data[2]= 1.50441
out_data[2]= 1.50417
out_data[2]= 1.50351
out_data[2]= 1.50247
out_data[2]= 1.50076
out_data[2]= 1.49853
out_data[2]= 1.49648
out_data[2]= 1.49495