diff options
| author | daregger <daregger@student.ethz.ch> | 2012-10-16 16:49:45 +0200 |
|---|---|---|
| committer | daregger <daregger@student.ethz.ch> | 2012-10-16 16:49:45 +0200 |
| commit | b50bc7798ac463de3e0c3147df46a3f7227df8c3 (patch) | |
| tree | 7a7823cf52632136814214509f8f1d07192b8915 /apps/attitude_estimator_ekf | |
| parent | 0b26ca84d451adfdf80e956fc1b199def17aafd9 (diff) | |
| download | px4-firmware-b50bc7798ac463de3e0c3147df46a3f7227df8c3.tar.gz px4-firmware-b50bc7798ac463de3e0c3147df46a3f7227df8c3.tar.bz2 px4-firmware-b50bc7798ac463de3e0c3147df46a3f7227df8c3.zip | |
Wip on inner rate loop
Diffstat (limited to 'apps/attitude_estimator_ekf')
| -rw-r--r--[-rwxr-xr-x] | apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c | 9 |
1 files changed, 5 insertions, 4 deletions
diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index b507b4c10..a291a4914 100755..100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -293,12 +293,13 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) gyro_offsets[0] += raw.gyro_rad_s[0]; gyro_offsets[1] += raw.gyro_rad_s[1]; gyro_offsets[2] += raw.gyro_rad_s[2]; - + offset_count+=1; if (hrt_absolute_time() - start_time > 3000000LL) { initialized = true; gyro_offsets[0] /= offset_count; gyro_offsets[1] /= offset_count; gyro_offsets[2] /= offset_count; + printf("pipapo %d\n",(int)(gyro_offsets[2]*1000) ); } } else { @@ -315,9 +316,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) sensor_last_timestamp[0] = raw.timestamp; } - z_k[0] = raw.gyro_rad_s[0]; - z_k[1] = raw.gyro_rad_s[1]; - z_k[2] = raw.gyro_rad_s[2]; + z_k[0] = raw.gyro_rad_s[0]-gyro_offsets[0]; + z_k[1] = raw.gyro_rad_s[1]-gyro_offsets[1]; + z_k[2] = raw.gyro_rad_s[2]-gyro_offsets[2]; /* update accelerometer measurements */ if (sensor_last_count[1] != raw.accelerometer_counter) { |