From b50bc7798ac463de3e0c3147df46a3f7227df8c3 Mon Sep 17 00:00:00 2001 From: daregger Date: Tue, 16 Oct 2012 16:49:45 +0200 Subject: Wip on inner rate loop --- apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) mode change 100755 => 100644 apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c (limited to 'apps/attitude_estimator_ekf') diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c old mode 100755 new mode 100644 index b507b4c10..a291a4914 --- 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) { -- cgit v1.2.3