Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-06 06:43:21 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

Fixed stupid interface bugs, working

Browse Source
This commit is contained in:
Lorenz Meier
2012-09-21 17:19:28 +02:00
parent 8a11f76994
commit 1d96f0b853
4 changed files with 15 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
+8 -7
View File
@@ -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
+4 -4
View File
@@ -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
+2 -2
View File
@@ -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
+1 -1
View File
@@ -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);