Fixed stupid interface bugs, working

apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c

@@ -245,8 +245,8 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 
 			/* scale from 14 bit to m/s2 */
 			z_k[3] = raw.accelerometer_m_s2[0];
-			z_k[4] = raw.accelerometer_raw[1];
-			z_k[5] = raw.accelerometer_raw[2];
+			z_k[4] = raw.accelerometer_m_s2[1];
+			z_k[5] = raw.accelerometer_m_s2[2];
 
 			z_k[6] = raw.magnetometer_ga[0];
 			z_k[7] = raw.magnetometer_ga[1];
@@ -274,8 +274,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 			static bool const_initialized = false;
 
 			/* initialize with good values once we have a reasonable dt estimate */
-			if (!const_initialized && dt < 0.05 && dt > 0.005) 
+			if (!const_initialized /*&& dt < 0.05 && dt > 0.005*/) 
 			{
+				dt = 0.005f;
 				knownConst[0] = powf(0.6f, 2.0f*dt);
 				knownConst[1] = powf(0.6f, 2.0f*dt);
 				knownConst[2] = powf(0.2f, 2.0f*dt);
@@ -308,7 +309,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 
 			// printcounter++;
 
-			if (last_data > 0 && raw.timestamp - last_data > 8000) printf("sensor data missed! (%llu)\n", raw.timestamp - last_data);
+			if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data);
 
 			last_data = raw.timestamp;
 
@@ -318,9 +319,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 			att.pitch = euler[1];
 			att.yaw = euler[2];
 
-			// att.rollspeed = rates.x;
-			// att.pitchspeed = rates.y;
-			// att.yawspeed = rates.z;
+			att.rollspeed = x_aposteriori[0];
+			att.pitchspeed = x_aposteriori[1];
+			att.yawspeed = x_aposteriori[2];
 
 			// Broadcast
 			orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);

apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c

@@ -35,7 +35,7 @@ static real32_T rt_atan2f_snf(real32_T u0, real32_T u1)
   if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
     y = ((real32_T)rtNaN);
   } else if (rtIsInfF(u0) && rtIsInfF(u1)) {
-    y = (real32_T)atan2(u0 > 0.0F ? 1.0F : -1.0F, u1 > 0.0F ? 1.0F : -1.0F);
+    y = (real32_T)atan2f(u0 > 0.0F ? 1.0F : -1.0F, u1 > 0.0F ? 1.0F : -1.0F);
   } else if (u1 == 0.0F) {
     if (u0 > 0.0F) {
       y = RT_PIF / 2.0F;
@@ -45,7 +45,7 @@ static real32_T rt_atan2f_snf(real32_T u0, real32_T u1)
       y = 0.0F;
     }
   } else {
-    y = (real32_T)atan2(u0, u1);
+    y = (real32_T)atan2f(u0, u1);
   }
 
   return y;
@@ -484,7 +484,7 @@ void attitudeKalmanfilter(real32_T dt, const int8_T updVect[9], const real32_T
         x_n_b[ib] = z_k_data[ib + 3];
       }
 
-      if ((real32_T)fabs(norm(x_n_b) - knownConst[11]) > knownConst[13]) {
+      if ((real32_T)fabsf(norm(x_n_b) - knownConst[11]) > knownConst[13]) {
         /* 'attitudeKalmanfilter:145' accUpt=10000; */
         accUpt = 10000;
       }
@@ -709,7 +709,7 @@ void attitudeKalmanfilter(real32_T dt, const int8_T updVect[9], const real32_T
   /* 'attitudeKalmanfilter:194' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */
   /* 'attitudeKalmanfilter:195' eulerAngles=[phi;theta;psi]; */
   eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]);
-  eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]);
+  eulerAngles[1] = -(real32_T)asinf(Rot_matrix[6]);
   eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]);
 }
 

apps/attitude_estimator_ekf/codegen/norm.c

@@ -40,7 +40,7 @@ real32_T norm(const real32_T x[3])
   firstNonZero = TRUE;
   for (k = 0; k < 3; k++) {
     if (x[k] != 0.0F) {
-      absxk = (real32_T)fabs(x[k]);
+      absxk = (real32_T)fabsf(x[k]);
       if (firstNonZero) {
         scale = absxk;
         y = 1.0F;
@@ -56,7 +56,7 @@ real32_T norm(const real32_T x[3])
     }
   }
 
-  return scale * (real32_T)sqrt(y);
+  return scale * (real32_T)sqrtf(y);
 }
 
 /* End of code generation (norm.c) */

apps/px4/ground_estimator/ground_estimator.c

@@ -97,7 +97,7 @@ int ground_estimator_thread_main(int argc, char *argv[]) {
 			/* XXX this is seriously bad - should be an emergency */
 		} else if (ret == 0) {
 			/* XXX this means no sensor data - should be critical or emergency */
-			printf("[attitude estimator bm] WARNING: Not getting sensor data - sensor app running?\n");
+			printf("[ground estimator bm] WARNING: Not getting sensor data - sensor app running?\n");
 		} else {
 			struct sensor_combined_s s;
 			orb_copy(ORB_ID(sensor_combined), sub_raw, &s);