diff options
4 files changed, 15 insertions, 14 deletions
diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index d04556e12..9ae4c50b7 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/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); diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c index bfbb3be01..459dcc9b9 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c +++ b/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]);
}
diff --git a/apps/attitude_estimator_ekf/codegen/norm.c b/apps/attitude_estimator_ekf/codegen/norm.c index 97509a0c0..fa07e1a90 100755 --- a/apps/attitude_estimator_ekf/codegen/norm.c +++ b/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) */
diff --git a/apps/px4/ground_estimator/ground_estimator.c b/apps/px4/ground_estimator/ground_estimator.c index 07426ec2b..ccf9ee3ec 100644 --- a/apps/px4/ground_estimator/ground_estimator.c +++ b/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); |