Browse Source

Fixed stupid interface bugs, working

sbg
Lorenz Meier 13 years ago
parent
commit
1d96f0b853
  1. 15
      apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
  2. 8
      apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
  3. 4
      apps/attitude_estimator_ekf/codegen/norm.c
  4. 2
      apps/px4/ground_estimator/ground_estimator.c

15
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 */ /* scale from 14 bit to m/s2 */
z_k[3] = raw.accelerometer_m_s2[0]; z_k[3] = raw.accelerometer_m_s2[0];
z_k[4] = raw.accelerometer_raw[1]; z_k[4] = raw.accelerometer_m_s2[1];
z_k[5] = raw.accelerometer_raw[2]; z_k[5] = raw.accelerometer_m_s2[2];
z_k[6] = raw.magnetometer_ga[0]; z_k[6] = raw.magnetometer_ga[0];
z_k[7] = raw.magnetometer_ga[1]; 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; static bool const_initialized = false;
/* initialize with good values once we have a reasonable dt estimate */ /* 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[0] = powf(0.6f, 2.0f*dt);
knownConst[1] = powf(0.6f, 2.0f*dt); knownConst[1] = powf(0.6f, 2.0f*dt);
knownConst[2] = powf(0.2f, 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++; // 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; last_data = raw.timestamp;
@ -318,9 +319,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
att.pitch = euler[1]; att.pitch = euler[1];
att.yaw = euler[2]; att.yaw = euler[2];
// att.rollspeed = rates.x; att.rollspeed = x_aposteriori[0];
// att.pitchspeed = rates.y; att.pitchspeed = x_aposteriori[1];
// att.yawspeed = rates.z; att.yawspeed = x_aposteriori[2];
// Broadcast // Broadcast
orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);

8
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)) { if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
y = ((real32_T)rtNaN); y = ((real32_T)rtNaN);
} else if (rtIsInfF(u0) && rtIsInfF(u1)) { } 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) { } else if (u1 == 0.0F) {
if (u0 > 0.0F) { if (u0 > 0.0F) {
y = RT_PIF / 2.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; y = 0.0F;
} }
} else { } else {
y = (real32_T)atan2(u0, u1); y = (real32_T)atan2f(u0, u1);
} }
return y; 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]; 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; */ /* 'attitudeKalmanfilter:145' accUpt=10000; */
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:194' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */
/* 'attitudeKalmanfilter:195' eulerAngles=[phi;theta;psi]; */ /* 'attitudeKalmanfilter:195' eulerAngles=[phi;theta;psi]; */
eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]); 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]); eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]);
} }

4
apps/attitude_estimator_ekf/codegen/norm.c

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

2
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 */ /* XXX this is seriously bad - should be an emergency */
} else if (ret == 0) { } else if (ret == 0) {
/* XXX this means no sensor data - should be critical or emergency */ /* 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 { } else {
struct sensor_combined_s s; struct sensor_combined_s s;
orb_copy(ORB_ID(sensor_combined), sub_raw, &s); orb_copy(ORB_ID(sensor_combined), sub_raw, &s);

Loading…
Cancel
Save