mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-25 16:56:25 +08:00
Updated EKF filter, fixed uploader (reverted to master)
This commit is contained in:
Lorenz Meier
+14
-3
@@ -263,8 +263,8 @@ class uploader(object):
|
||||
print("program...")
|
||||
self.__program(fw)
|
||||
|
||||
#print("verify...")
|
||||
#self.__verify(fw)
|
||||
print("verify...")
|
||||
self.__verify(fw)
|
||||
|
||||
print("done, rebooting.")
|
||||
self.__reboot()
|
||||
@@ -290,7 +290,18 @@ while True:
|
||||
|
||||
# create an uploader attached to the port
|
||||
try:
|
||||
up = uploader("\\\\.\\COM2", args.baud)
|
||||
if "linux" in _platform:
|
||||
# Linux, don't open Mac OS and Win ports
|
||||
if not "COM" in port and not "tty.usb" in port:
|
||||
up = uploader(port, args.baud)
|
||||
elif "darwin" in _platform:
|
||||
# OS X, don't open Windows and Linux ports
|
||||
if not "COM" in port and not "ACM" in port:
|
||||
up = uploader(port, args.baud)
|
||||
elif "win" in _platform:
|
||||
# Windows, don't open POSIX ports
|
||||
if not "/" in port:
|
||||
up = uploader(port, args.baud)
|
||||
except:
|
||||
# open failed, rate-limit our attempts
|
||||
time.sleep(0.05)
|
||||
|
||||
@@ -45,9 +45,9 @@ CSRCS = attitude_estimator_ekf_main.c \
|
||||
codegen/rtGetInf.c \
|
||||
codegen/rtGetNaN.c \
|
||||
codegen/norm.c \
|
||||
codegen/find.c \
|
||||
codegen/diag.c \
|
||||
codegen/cross.c
|
||||
codegen/cross.c \
|
||||
codegen/power.c
|
||||
|
||||
# XXX this is *horribly* broken
|
||||
INCLUDES += $(TOPDIR)/../mavlink/include/mavlink
|
||||
|
||||
@@ -75,38 +75,45 @@ __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]);
|
||||
|
||||
static unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
|
||||
|
||||
static float dt = 1;
|
||||
static float dt = 1.0f;
|
||||
/* 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,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,
|
||||
0, 0, 100.f, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 100.f, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 100.f, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 100.f, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 100.f, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 100.f, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 100.f,
|
||||
static float z_k[9]; /**< Measurement vector */
|
||||
static float x_aposteriori_k[12]; /**< */
|
||||
static float x_aposteriori[12];
|
||||
static float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f,
|
||||
};
|
||||
static float P_aposteriori[81] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 100.f, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 100.f, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 100.f, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 100.f, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 100.f, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 100.f, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 100.f, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 100.f,
|
||||
static float P_aposteriori[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f,
|
||||
}; /**< init: diagonal matrix with big values */
|
||||
static float knownConst[15] = {1, 1, 1, 1, 1, 0.04, 4, 0.1, 70, 70, -2000, 9.81, 1, 4, 1}; /**< knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */
|
||||
// static float knownConst[15] = {1, 1, 1, 1, 1, 0.04, 4, 0.1, 70, 70, -2000, 9.81, 1, 4, 1}; /**< knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */
|
||||
static float Rot_matrix[9] = {1.f, 0, 0,
|
||||
0, 1.f, 0,
|
||||
0, 0, 1.f
|
||||
}; /**< init: identity matrix */
|
||||
|
||||
|
||||
static bool thread_should_exit = false; /**< Deamon exit flag */
|
||||
static bool thread_running = false; /**< Deamon status flag */
|
||||
static int attitude_estimator_ekf_task; /**< Handle of deamon task / thread */
|
||||
@@ -229,6 +236,14 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
|
||||
struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
|
||||
orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg);
|
||||
|
||||
|
||||
/* process noise covariance */
|
||||
float q[12];
|
||||
/* measurement noise covariance */
|
||||
float r[9];
|
||||
/* output euler angles */
|
||||
float euler[3] = {0.0f, 0.0f, 0.0f};
|
||||
|
||||
/* Main loop*/
|
||||
while (!thread_should_exit) {
|
||||
|
||||
@@ -278,10 +293,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
|
||||
overloadcounter++;
|
||||
}
|
||||
|
||||
int8_t update_vect[3] = {1, 1, 1};
|
||||
float euler[3];
|
||||
uint8_t update_vect[3] = {1, 1, 1};
|
||||
int32_t z_k_sizes = 9;
|
||||
float u[4] = {0.0f, 0.0f, 0.0f, 0.0f};
|
||||
// float u[4] = {0.0f, 0.0f, 0.0f, 0.0f};
|
||||
|
||||
static bool const_initialized = false;
|
||||
|
||||
@@ -289,38 +303,41 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
|
||||
if (!const_initialized /*&& dt < 0.05 && dt > 0.005*/)
|
||||
{
|
||||
dt = 0.005f;
|
||||
q[0] = 1e1;
|
||||
q[1] = 1e1;
|
||||
q[2] = 1e1;
|
||||
q[3] = 1e-6;
|
||||
q[4] = 1e-6;
|
||||
q[5] = 1e-6;
|
||||
q[6] = 1e-1;
|
||||
q[7] = 1e-1;
|
||||
q[8] = 1e-1;
|
||||
q[9] = 1e-1;
|
||||
q[10] = 1e-1;
|
||||
q[11] = 1e-1;
|
||||
q[0] = 1e1f;
|
||||
q[1] = 1e1f;
|
||||
q[2] = 1e1f;
|
||||
q[3] = 1e-6f;
|
||||
q[4] = 1e-6f;
|
||||
q[5] = 1e-6f;
|
||||
q[6] = 1e-1f;
|
||||
q[7] = 1e-1f;
|
||||
q[8] = 1e-1f;
|
||||
q[9] = 1e-1f;
|
||||
q[10] = 1e-1f;
|
||||
q[11] = 1e-1f;
|
||||
|
||||
r[0]= 1e-2;
|
||||
r[1]= 1e-2;
|
||||
r[2]= 1e-2;
|
||||
r[3]= 1e-1;
|
||||
r[4]= 1e-1;
|
||||
r[5]= 1e-1;
|
||||
r[6]= 1e-1;
|
||||
r[7]= 1e-1;
|
||||
r[8]= 1e-1;
|
||||
r[0]= 1e-2f;
|
||||
r[1]= 1e-2f;
|
||||
r[2]= 1e-2f;
|
||||
r[3]= 1e-1f;
|
||||
r[4]= 1e-1f;
|
||||
r[5]= 1e-1f;
|
||||
r[6]= 1e-1f;
|
||||
r[7]= 1e-1f;
|
||||
r[8]= 1e-1f;
|
||||
|
||||
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];
|
||||
x_aposteriori_k[3] = 0.0f;
|
||||
x_aposteriori_k[4] = 0.0f;
|
||||
x_aposteriori_k[5] = 0.0f;
|
||||
x_aposteriori_k[6] = z_k[3];
|
||||
x_aposteriori_k[7] = z_k[4];
|
||||
x_aposteriori_k[8] = z_k[5];
|
||||
x_aposteriori_k[9] = z_k[6];
|
||||
x_aposteriori_k[10] = z_k[7];
|
||||
x_aposteriori_k[11] = z_k[8];
|
||||
|
||||
const_initialized = true;
|
||||
}
|
||||
@@ -331,15 +348,17 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
|
||||
}
|
||||
|
||||
uint64_t timing_start = hrt_absolute_time();
|
||||
attitudeKalmanfilter(dt, update_vect, z_k, &z_k_sizes, u, x_aposteriori_k, P_aposteriori_k, knownConst, euler,
|
||||
Rot_matrix, x_aposteriori, P_aposteriori);
|
||||
// attitudeKalmanfilter(dt, update_vect, z_k, &z_k_sizes, u, x_aposteriori_k, P_aposteriori_k, knownConst, euler,
|
||||
// Rot_matrix, x_aposteriori, P_aposteriori);
|
||||
attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, q, r,
|
||||
euler, Rot_matrix, x_aposteriori, P_aposteriori);
|
||||
/* swap values for next iteration */
|
||||
memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k));
|
||||
memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k));
|
||||
uint64_t timing_diff = hrt_absolute_time() - timing_start;
|
||||
|
||||
/* print rotation matrix every 200th time */
|
||||
if (printcounter % 2 == 0) {
|
||||
if (printcounter % 200 == 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],
|
||||
@@ -348,8 +367,8 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
|
||||
|
||||
// }
|
||||
|
||||
// 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("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\n", (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));
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:43 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
@@ -29,6 +29,6 @@
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
extern void attitudeKalmanfilter(real32_T dt, const int8_T updVect[9], const real32_T z_k_data[9], const int32_T z_k_sizes[1], const real32_T u[4], const real32_T x_aposteriori_k[9], const real32_T P_aposteriori_k[81], const real32_T knownConst[20], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[9], real32_T P_aposteriori[81]);
|
||||
extern void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T q[12], const real32_T r[9], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]);
|
||||
#endif
|
||||
/* End of code generation (attitudeKalmanfilter.h) */
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter_initialize'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:44 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter_initialize'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:44 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter_terminate'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:44 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter_terminate'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:44 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:42 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'cross'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:44 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'cross'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:44 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'diag'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:43 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
@@ -27,7 +27,19 @@
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void diag(const real32_T v[3], real32_T d[9])
|
||||
void b_diag(const real32_T v[9], real32_T d[81])
|
||||
{
|
||||
int32_T j;
|
||||
memset((void *)&d[0], 0, 81U * sizeof(real32_T));
|
||||
for (j = 0; j < 9; j++) {
|
||||
d[j + 9 * j] = v[j];
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void c_diag(const real32_T v[3], real32_T d[9])
|
||||
{
|
||||
int32_T j;
|
||||
for (j = 0; j < 9; j++) {
|
||||
@@ -39,4 +51,28 @@ void diag(const real32_T v[3], real32_T d[9])
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void d_diag(const real32_T v[6], real32_T d[36])
|
||||
{
|
||||
int32_T j;
|
||||
memset((void *)&d[0], 0, 36U * sizeof(real32_T));
|
||||
for (j = 0; j < 6; j++) {
|
||||
d[j + 6 * j] = v[j];
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void diag(const real32_T v[12], real32_T d[144])
|
||||
{
|
||||
int32_T j;
|
||||
memset((void *)&d[0], 0, 144U * sizeof(real32_T));
|
||||
for (j = 0; j < 12; j++) {
|
||||
d[j + 12 * j] = v[j];
|
||||
}
|
||||
}
|
||||
|
||||
/* End of code generation (diag.c) */
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'diag'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:43 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
@@ -29,6 +29,9 @@
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
extern void diag(const real32_T v[3], real32_T d[9]);
|
||||
extern void b_diag(const real32_T v[9], real32_T d[81]);
|
||||
extern void c_diag(const real32_T v[3], real32_T d[9]);
|
||||
extern void d_diag(const real32_T v[6], real32_T d[36]);
|
||||
extern void diag(const real32_T v[12], real32_T d[144]);
|
||||
#endif
|
||||
/* End of code generation (diag.h) */
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'eye'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:43 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
@@ -27,12 +27,12 @@
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void b_eye(real_T I[81])
|
||||
void b_eye(real_T I[144])
|
||||
{
|
||||
int32_T i;
|
||||
memset((void *)&I[0], 0, 81U * sizeof(real_T));
|
||||
for (i = 0; i < 9; i++) {
|
||||
I[i + 9 * i] = 1.0;
|
||||
memset((void *)&I[0], 0, 144U * sizeof(real_T));
|
||||
for (i = 0; i < 12; i++) {
|
||||
I[i + 12 * i] = 1.0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'eye'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:43 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
@@ -29,7 +29,7 @@
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
extern void b_eye(real_T I[81]);
|
||||
extern void b_eye(real_T I[144]);
|
||||
extern void eye(real_T I[9]);
|
||||
#endif
|
||||
/* End of code generation (eye.h) */
|
||||
|
||||
@@ -1,77 +0,0 @@
|
||||
/*
|
||||
* find.c
|
||||
*
|
||||
* Code generation for function 'find'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:43 2012
|
||||
*
|
||||
*/
|
||||
|
||||
/* Include files */
|
||||
#include "rt_nonfinite.h"
|
||||
#include "attitudeKalmanfilter.h"
|
||||
#include "find.h"
|
||||
|
||||
/* Type Definitions */
|
||||
|
||||
/* Named Constants */
|
||||
|
||||
/* Variable Declarations */
|
||||
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
|
||||
/* Function Definitions */
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void find(const int8_T x[9], real_T i_data[9], int32_T i_sizes[1])
|
||||
{
|
||||
int32_T idx;
|
||||
int32_T ii;
|
||||
boolean_T exitg1;
|
||||
boolean_T guard1 = FALSE;
|
||||
int32_T i2;
|
||||
int8_T b_i_data[9];
|
||||
idx = 0;
|
||||
i_sizes[0] = 9;
|
||||
ii = 1;
|
||||
exitg1 = 0U;
|
||||
while ((exitg1 == 0U) && (ii <= 9)) {
|
||||
guard1 = FALSE;
|
||||
if (x[ii - 1] != 0) {
|
||||
idx++;
|
||||
i_data[idx - 1] = (real_T)ii;
|
||||
if (idx >= 9) {
|
||||
exitg1 = 1U;
|
||||
} else {
|
||||
guard1 = TRUE;
|
||||
}
|
||||
} else {
|
||||
guard1 = TRUE;
|
||||
}
|
||||
|
||||
if (guard1 == TRUE) {
|
||||
ii++;
|
||||
}
|
||||
}
|
||||
|
||||
if (1 > idx) {
|
||||
idx = 0;
|
||||
}
|
||||
|
||||
ii = idx - 1;
|
||||
for (i2 = 0; i2 <= ii; i2++) {
|
||||
b_i_data[i2] = (int8_T)i_data[i2];
|
||||
}
|
||||
|
||||
i_sizes[0] = idx;
|
||||
ii = idx - 1;
|
||||
for (i2 = 0; i2 <= ii; i2++) {
|
||||
i_data[i2] = (real_T)b_i_data[i2];
|
||||
}
|
||||
}
|
||||
|
||||
/* End of code generation (find.c) */
|
||||
File diff suppressed because it is too large
Load Diff
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'mrdivide'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:44 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
@@ -29,6 +29,8 @@
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
extern void mrdivide(const real32_T A_data[81], const int32_T A_sizes[2], const real32_T B_data[81], const int32_T B_sizes[2], real32_T y_data[81], int32_T y_sizes[2]);
|
||||
extern void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]);
|
||||
extern void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]);
|
||||
extern void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]);
|
||||
#endif
|
||||
/* End of code generation (mrdivide.h) */
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'norm'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:43 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'norm'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:44 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
Executable
+84
@@ -0,0 +1,84 @@
|
||||
/*
|
||||
* power.c
|
||||
*
|
||||
* Code generation for function 'power'
|
||||
*
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
/* Include files */
|
||||
#include "rt_nonfinite.h"
|
||||
#include "attitudeKalmanfilter.h"
|
||||
#include "power.h"
|
||||
|
||||
/* Type Definitions */
|
||||
|
||||
/* Named Constants */
|
||||
|
||||
/* Variable Declarations */
|
||||
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
static real32_T rt_powf_snf(real32_T u0, real32_T u1);
|
||||
|
||||
/* Function Definitions */
|
||||
static real32_T rt_powf_snf(real32_T u0, real32_T u1)
|
||||
{
|
||||
real32_T y;
|
||||
real32_T f0;
|
||||
real32_T f1;
|
||||
if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
|
||||
y = ((real32_T)rtNaN);
|
||||
} else {
|
||||
f0 = (real32_T)fabsf(u0);
|
||||
f1 = (real32_T)fabsf(u1);
|
||||
if (rtIsInfF(u1)) {
|
||||
if (f0 == 1.0F) {
|
||||
y = ((real32_T)rtNaN);
|
||||
} else if (f0 > 1.0F) {
|
||||
if (u1 > 0.0F) {
|
||||
y = ((real32_T)rtInf);
|
||||
} else {
|
||||
y = 0.0F;
|
||||
}
|
||||
} else if (u1 > 0.0F) {
|
||||
y = 0.0F;
|
||||
} else {
|
||||
y = ((real32_T)rtInf);
|
||||
}
|
||||
} else if (f1 == 0.0F) {
|
||||
y = 1.0F;
|
||||
} else if (f1 == 1.0F) {
|
||||
if (u1 > 0.0F) {
|
||||
y = u0;
|
||||
} else {
|
||||
y = 1.0F / u0;
|
||||
}
|
||||
} else if (u1 == 2.0F) {
|
||||
y = u0 * u0;
|
||||
} else if ((u1 == 0.5F) && (u0 >= 0.0F)) {
|
||||
y = (real32_T)sqrtf(u0);
|
||||
} else if ((u0 < 0.0F) && (u1 > (real32_T)floorf(u1))) {
|
||||
y = ((real32_T)rtNaN);
|
||||
} else {
|
||||
y = (real32_T)powf(u0, u1);
|
||||
}
|
||||
}
|
||||
|
||||
return y;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*/
|
||||
void power(const real32_T a[12], real_T b, real32_T y[12])
|
||||
{
|
||||
int32_T k;
|
||||
for (k = 0; k < 12; k++) {
|
||||
y[k] = rt_powf_snf(a[k], (real32_T)b);
|
||||
}
|
||||
}
|
||||
|
||||
/* End of code generation (power.c) */
|
||||
+7
-7
@@ -1,14 +1,14 @@
|
||||
/*
|
||||
* find.h
|
||||
* power.h
|
||||
*
|
||||
* Code generation for function 'find'
|
||||
* Code generation for function 'power'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:43 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __FIND_H__
|
||||
#define __FIND_H__
|
||||
#ifndef __POWER_H__
|
||||
#define __POWER_H__
|
||||
/* Include files */
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
@@ -29,6 +29,6 @@
|
||||
/* Variable Definitions */
|
||||
|
||||
/* Function Declarations */
|
||||
extern void find(const int8_T x[9], real_T i_data[9], int32_T i_sizes[1]);
|
||||
extern void power(const real32_T a[12], real_T b, real32_T y[12]);
|
||||
#endif
|
||||
/* End of code generation (find.h) */
|
||||
/* End of code generation (power.h) */
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:45 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:45 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:45 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:44 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:44 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:44 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:44 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
*
|
||||
* Code generation for function 'attitudeKalmanfilter'
|
||||
*
|
||||
* C source code generated on: Fri Sep 21 13:56:44 2012
|
||||
* C source code generated on: Mon Oct 01 19:38:49 2012
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
Reference in New Issue
Block a user