dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Debugging output still enabled, fixed a number of additional issues

This commit is contained in:
Lorenz Meier 2012-09-21 19:14:50 +02:00
parent 1d96f0b853
commit 6c7e21bd1c
2 changed files with 44 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
View File

@ -55,6 +55,7 @@
#include <limits.h>
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <arch/board/up_hrt.h>
@ -76,7 +77,7 @@ static float dt = 1;
/* 0, 0, -9.81, 1, 1, 1, wo (gyro offset), w */
/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
static float z_k[9] = {0}; /**< Measurement vector */
static float x_aposteriori_k[9] = {0}; /**< */
static float x_aposteriori_k[9] = {0,0,0,0,0,-9.81,1,1,-1}; /**< */
static float x_aposteriori[9] = {0};
static float P_aposteriori_k[81] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0,
0, 100.f, 0, 0, 0, 0, 0, 0, 0,
@ -217,6 +218,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
thread_running = true;
/* advertise debug value */
struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg);
/* Main loop*/
while (!thread_should_exit) {
@ -266,7 +271,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
overloadcounter++;
}
int8_t update_vect[9] = {1, 1, 1, 1, 1, 1, 1, 1, 1};
int8_t update_vect[9] = {1, 1, 1, 1, 1, 1, 0, 0, 0};
float euler[3];
int32_t z_k_sizes = 9;
float u[4] = {0.0f, 0.0f, 0.0f, 0.0f};
@ -277,11 +282,22 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
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);
knownConst[3] = powf(0.2f, 2.0f*dt);
knownConst[4] = powf(0.000001f, 2.0f*dt);
knownConst[0] = 0.6f*0.6f*dt;
knownConst[1] = 0.6f*0.6f*dt;
knownConst[2] = 0.2f*0.2f*dt;
knownConst[3] = 0.2f*0.2f*dt;
knownConst[4] = 0.000001f*0.000001f*dt; // -9.81,1,1,-1};
x_aposteriori_k[0] = z_k[0];
x_aposteriori_k[1] = z_k[1];
x_aposteriori_k[2] = z_k[2];
x_aposteriori_k[3] = z_k[3];
x_aposteriori_k[4] = z_k[4];
x_aposteriori_k[5] = z_k[5];
x_aposteriori_k[6] = z_k[6];
x_aposteriori_k[7] = z_k[7];
x_aposteriori_k[8] = z_k[8];
const_initialized = true;
}
@ -299,15 +315,32 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
uint64_t timing_diff = hrt_absolute_time() - timing_start;
/* print rotation matrix every 200th time */
// if (printcounter % 200 == 0) {
if (printcounter % 2 == 0) {
// printf("x apo:\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n",
// x_aposteriori[0], x_aposteriori[1], x_aposteriori[2],
// x_aposteriori[3], x_aposteriori[4], x_aposteriori[5],
// x_aposteriori[6], x_aposteriori[7], x_aposteriori[8]);
// }
// printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt));
// printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f", (double)euler[0], (double)euler[1], (double)euler[2]);
// printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100),
// (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100),
// (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100));
// }
}
// printcounter++;
int i = printcounter % 9;
// for (int i = 0; i < 9; i++) {
char name[10];
sprintf(name, "xapo #%d", i);
memcpy(dbg.key, name, sizeof(dbg.key));
dbg.value = x_aposteriori[i];
orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg);
printcounter++;
if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data);

2
apps/mavlink/mavlink.c
View File

@ -1664,7 +1664,7 @@ int mavlink_thread_main(int argc, char *argv[])
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 5);
/* 200 Hz / 5 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 5);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 5);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 3);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 5);
/* 5 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 200);