From 8a365179eafdf3aea98e60ab9f5882b200d4c759 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 4 Aug 2012 15:12:36 -0700 Subject: Fresh import of the PX4 firmware sources. --- apps/attitude_estimator_ekf/.context | 0 apps/attitude_estimator_ekf/Makefile | 52 ++ apps/attitude_estimator_ekf/attitudeKalmanfilter.m | 108 ++++ .../attitudeKalmanfilter.prj | 270 +++++++++ .../attitude_estimator_ekf_main.c | 263 +++++++++ .../codegen/attitudeKalmanfilter.c | 635 +++++++++++++++++++++ .../codegen/attitudeKalmanfilter.h | 32 ++ .../codegen/attitudeKalmanfilter_initialize.c | 31 + .../codegen/attitudeKalmanfilter_initialize.h | 32 ++ .../codegen/attitudeKalmanfilter_ref.rsp | 0 .../codegen/attitudeKalmanfilter_rtw.bat | 11 + .../codegen/attitudeKalmanfilter_rtw.mk | 328 +++++++++++ .../codegen/attitudeKalmanfilter_terminate.c | 31 + .../codegen/attitudeKalmanfilter_terminate.h | 32 ++ .../codegen/attitudeKalmanfilter_types.h | 16 + apps/attitude_estimator_ekf/codegen/buildInfo.mat | Bin 0 -> 4114 bytes apps/attitude_estimator_ekf/codegen/eye.c | 51 ++ apps/attitude_estimator_ekf/codegen/eye.h | 33 ++ apps/attitude_estimator_ekf/codegen/mrdivide.c | 158 +++++ apps/attitude_estimator_ekf/codegen/mrdivide.h | 32 ++ apps/attitude_estimator_ekf/codegen/norm.c | 62 ++ apps/attitude_estimator_ekf/codegen/norm.h | 32 ++ apps/attitude_estimator_ekf/codegen/rtGetInf.c | 139 +++++ apps/attitude_estimator_ekf/codegen/rtGetInf.h | 23 + apps/attitude_estimator_ekf/codegen/rtGetNaN.c | 96 ++++ apps/attitude_estimator_ekf/codegen/rtGetNaN.h | 21 + apps/attitude_estimator_ekf/codegen/rt_nonfinite.c | 87 +++ apps/attitude_estimator_ekf/codegen/rt_nonfinite.h | 53 ++ apps/attitude_estimator_ekf/codegen/rtw_proj.tmw | 1 + apps/attitude_estimator_ekf/codegen/rtwtypes.h | 159 ++++++ 30 files changed, 2788 insertions(+) create mode 100644 apps/attitude_estimator_ekf/.context create mode 100644 apps/attitude_estimator_ekf/Makefile create mode 100755 apps/attitude_estimator_ekf/attitudeKalmanfilter.m create mode 100755 apps/attitude_estimator_ekf/attitudeKalmanfilter.prj create mode 100644 apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c create mode 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c create mode 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h create mode 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c create mode 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h create mode 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_ref.rsp create mode 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.bat create mode 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.mk create mode 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c create mode 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h create mode 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h create mode 100755 apps/attitude_estimator_ekf/codegen/buildInfo.mat create mode 100755 apps/attitude_estimator_ekf/codegen/eye.c create mode 100755 apps/attitude_estimator_ekf/codegen/eye.h create mode 100755 apps/attitude_estimator_ekf/codegen/mrdivide.c create mode 100755 apps/attitude_estimator_ekf/codegen/mrdivide.h create mode 100755 apps/attitude_estimator_ekf/codegen/norm.c create mode 100755 apps/attitude_estimator_ekf/codegen/norm.h create mode 100755 apps/attitude_estimator_ekf/codegen/rtGetInf.c create mode 100755 apps/attitude_estimator_ekf/codegen/rtGetInf.h create mode 100755 apps/attitude_estimator_ekf/codegen/rtGetNaN.c create mode 100755 apps/attitude_estimator_ekf/codegen/rtGetNaN.h create mode 100755 apps/attitude_estimator_ekf/codegen/rt_nonfinite.c create mode 100755 apps/attitude_estimator_ekf/codegen/rt_nonfinite.h create mode 100755 apps/attitude_estimator_ekf/codegen/rtw_proj.tmw create mode 100755 apps/attitude_estimator_ekf/codegen/rtwtypes.h (limited to 'apps/attitude_estimator_ekf') diff --git a/apps/attitude_estimator_ekf/.context b/apps/attitude_estimator_ekf/.context new file mode 100644 index 000000000..e69de29bb diff --git a/apps/attitude_estimator_ekf/Makefile b/apps/attitude_estimator_ekf/Makefile new file mode 100644 index 000000000..cad20d375 --- /dev/null +++ b/apps/attitude_estimator_ekf/Makefile @@ -0,0 +1,52 @@ +############################################################################ +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +APPNAME = attitude_estimator_ekf +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 20000 + +CSRCS = attitude_estimator_ekf_main.c \ + codegen/eye.c \ + codegen/attitudeKalmanfilter.c \ + codegen/mrdivide.c \ + codegen/attitudeKalmanfilter_initialize.c \ + codegen/attitudeKalmanfilter_terminate.c \ + codegen/rt_nonfinite.c \ + codegen/rtGetInf.c \ + codegen/rtGetNaN.c \ + codegen/norm.c + +# XXX this is *horribly* broken +INCLUDES += $(TOPDIR)/../mavlink/include/mavlink + +include $(APPDIR)/mk/app.mk diff --git a/apps/attitude_estimator_ekf/attitudeKalmanfilter.m b/apps/attitude_estimator_ekf/attitudeKalmanfilter.m new file mode 100755 index 000000000..5fb4aa94f --- /dev/null +++ b/apps/attitude_estimator_ekf/attitudeKalmanfilter.m @@ -0,0 +1,108 @@ +function [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) +%#codegen + + +%Extended Attitude Kalmanfilter +% + %state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' + %measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]' + %knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] + % + %[x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) + % + %Example.... + % + % $Author: Tobias Naegeli $ $Date: 2012 $ $Revision: 1 $ + + + %%define the matrices + acc_ProcessNoise=knownConst(1); + mag_ProcessNoise=knownConst(2); + ratesOffset_ProcessNoise=knownConst(3); + rates_ProcessNoise=knownConst(4); + + + acc_MeasurementNoise=knownConst(5); + mag_MeasurementNoise=knownConst(6); + rates_MeasurementNoise=knownConst(7); + + %process noise covariance matrix + Q = [ eye(3)*acc_ProcessNoise, zeros(3), zeros(3), zeros(3); + zeros(3), eye(3)*mag_ProcessNoise, zeros(3), zeros(3); + zeros(3), zeros(3), eye(3)*ratesOffset_ProcessNoise, zeros(3); + zeros(3), zeros(3), zeros(3), eye(3)*rates_ProcessNoise]; + + %measurement noise covariance matrix + R = [ eye(3)*acc_MeasurementNoise, zeros(3), zeros(3); + zeros(3), eye(3)*mag_MeasurementNoise, zeros(3); + zeros(3), zeros(3), eye(3)*rates_MeasurementNoise]; + + + %observation matrix + H_k=[ eye(3), zeros(3), zeros(3), zeros(3); + zeros(3), eye(3), zeros(3), zeros(3); + zeros(3), zeros(3), eye(3), eye(3)]; + + %compute A(t,w) + + %x_aposteriori_k[10,11,12] should be [p,q,r] + %R_temp=[1,-r, q + % r, 1, -p + % -q, p, 1] + + R_temp=[1,-dt*x_aposteriori_k(12),dt*x_aposteriori_k(11); + dt*x_aposteriori_k(12),1,-dt*x_aposteriori_k(10); + -dt*x_aposteriori_k(11), dt*x_aposteriori_k(10),1]; + + %strange, should not be transposed + A_pred=[R_temp', zeros(3), zeros(3), zeros(3); + zeros(3), R_temp', zeros(3), zeros(3); + zeros(3), zeros(3), eye(3), zeros(3); + zeros(3), zeros(3), zeros(3), eye(3)]; + + %%prediction step + x_apriori=A_pred*x_aposteriori_k; + + %linearization + acc_temp_mat=[0, dt*x_aposteriori_k(3), -dt*x_aposteriori_k(2); + -dt*x_aposteriori_k(3), 0, dt*x_aposteriori_k(1); + dt*x_aposteriori_k(2), -dt*x_aposteriori_k(1), 0]; + + mag_temp_mat=[0, dt*x_aposteriori_k(6), -dt*x_aposteriori_k(5); + -dt*x_aposteriori_k(6), 0, dt*x_aposteriori_k(4); + dt*x_aposteriori_k(5), -dt*x_aposteriori_k(4), 0]; + + A_lin=[R_temp', zeros(3), zeros(3), acc_temp_mat'; + zeros(3), R_temp', zeros(3), mag_temp_mat'; + zeros(3), zeros(3), eye(3), zeros(3); + zeros(3), zeros(3), zeros(3), eye(3)]; + + + P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; + + + %%update step + + y_k=z_k-H_k*x_apriori; + S_k=H_k*P_apriori*H_k'+R; + K_k=(P_apriori*H_k'/(S_k)); + + + x_aposteriori=x_apriori+K_k*y_k; + P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; + + + %%Rotation matrix generation + + earth_z=x_aposteriori(1:3)/norm(x_aposteriori(1:3)); + earth_x=cross(earth_z,x_aposteriori(4:6)/norm(x_aposteriori(4:6))); + earth_y=cross(earth_x,earth_z); + + Rot_matrix=[earth_x,earth_y,earth_z]; + + + + + + + diff --git a/apps/attitude_estimator_ekf/attitudeKalmanfilter.prj b/apps/attitude_estimator_ekf/attitudeKalmanfilter.prj new file mode 100755 index 000000000..431ddb71e --- /dev/null +++ b/apps/attitude_estimator_ekf/attitudeKalmanfilter.prj @@ -0,0 +1,270 @@ + + + + option.general.TargetLang.C + true + true + true + true + true + true + true + option.general.FilePartitionMethod.MapMFileToCFile + option.general.GlobalDataSyncMethod.SyncAlways + true + option.general.DynamicMemoryAllocation.Disabled + option.paths.working.project + + option.paths.build.project + + + true + false + true + true + + + + + + + + + + true + 64 + true + 10 + 200 + 4000 + 200000 + 10000 + option.general.TargetLang.C + MATLAB Embedded Coder Target + option.CCompilerOptimization.On + + true + make_rtw + default_tmf + true + option.general.FilePartitionMethod.MapMFileToCFile + true + option.general.DynamicMemoryAllocation.Disabled + option.paths.working.project + + option.paths.build.project + + + true + false + true + true + false + $M$N + $M$N + $M$N + $M$N + $M$N + $M$N + emxArray_$M$N + 32 + + + + + + + + + + false + C89/C90 (ANSI) + true + C99 (ISO) + false + true + true + false + option.ParenthesesLevel.Nominal + false + true + true + 64 + true + 10 + 200 + 4000 + 200000 + 10000 + true + attitudeKalmanfilter_mex + attitudeKalmanfilter + option.target.artifact.lib + false + true + R2011a + true + + F:\codegenerationMatlabAttFilter\codegen\lib\attitudeKalmanfilter\html\index.html + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + single + 1 x 1 + + + false + + + single + 9 x 1 + + + false + + + single + 12 x 1 + + + false + + + single + 12 x 12 + + + false + + + single + 7 x 1 + + + false + + + + + + + + C:\Program Files\MATLAB\R2011a + + + false + false + true + false + false + false + false + false + 6.1 + false + true + win64 + true + + + + diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c new file mode 100644 index 000000000..fdf6c9d91 --- /dev/null +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -0,0 +1,263 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: Tobias Naegeli + * Laurens Mackay + * Lorenz Meier + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file Extended Kalman Filter for Attitude Estimation + */ + + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "codegen/attitudeKalmanfilter_initialize.h" +#include "codegen/attitudeKalmanfilter.h" + +__EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]); + + +// #define N_STATES 6 + +// #define PROJECTION_INITIALIZE_COUNTER_LIMIT 5000 +// #define REPROJECTION_COUNTER_LIMIT 125 + +static unsigned int loop_interval_alarm = 4500; // loop interval in microseconds + +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[12] = {0}; /**< */ +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, 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 + }; /**< init: diagonal matrix with big values */ +static float knownConst[7] = {1, 1, 1, 1, 1, 1, 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 float x_aposteriori_k[12] = {0}; +// static float P_aposteriori_k[144] = {0}; + +/* + * [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) + */ + +/* + * EKF Attitude Estimator main function. + * + * Estimates the attitude recursively once started. + * + * @param argc number of commandline arguments (plus command name) + * @param argv strings containing the arguments + */ +int attitude_estimator_ekf_main(int argc, char *argv[]) +{ + // print text + printf("Extended Kalman Filter Attitude Estimator initialized..\n\n"); + fflush(stdout); + + int overloadcounter = 19; + + /* Initialize filter */ + attitudeKalmanfilter_initialize(); + + /* store start time to guard against too slow update rates */ + uint64_t last_run = hrt_absolute_time(); + + struct sensor_combined_s raw = {0}; + struct vehicle_attitude_s att = {}; + + uint64_t last_data = 0; + uint64_t last_measurement = 0; + + /* subscribe to raw data */ + int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); + /* advertise attitude */ + int pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); + + + int loopcounter = 0; + int printcounter = 0; + + /* Main loop*/ + while (true) { + + struct pollfd fds[1] = { + { .fd = sub_raw, .events = POLLIN }, + }; + int ret = poll(fds, 1, 1000); + + /* check for a timeout */ + if (ret == 0) { + /* */ + + /* update successful, copy data on every 2nd run and execute filter */ + } else if (loopcounter & 0x01) { + + orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); + + /* Calculate data time difference in seconds */ + dt = (raw.timestamp - last_measurement) / 1000000.0f; + last_measurement = raw.timestamp; + + // XXX Read out accel range via SPI on init, assuming 4G range at 14 bit res here + float range_g = 4.0f; + float mag_offset[3] = {0}; + /* scale from 14 bit to m/s2 */ + z_k[3] = ((raw.accelerometer_raw[0] * range_g) / 8192.0f) / 9.81f; // = accel * (1 / 32768.0f / 8.0f * 9.81f); + z_k[4] = ((raw.accelerometer_raw[1] * range_g) / 8192.0f) / 9.81f; // = accel * (1 / 32768.0f / 8.0f * 9.81f); + z_k[5] = ((raw.accelerometer_raw[2] * range_g) / 8192.0f) / 9.81f; // = accel * (1 / 32768.0f / 8.0f * 9.81f); + + // XXX Read out mag range via I2C on init, assuming 0.88 Ga and 12 bit res here + z_k[0] = (raw.magnetometer_raw[0] - mag_offset[0]) * 0.01f; + z_k[1] = (raw.magnetometer_raw[1] - mag_offset[1]) * 0.01f; + z_k[2] = (raw.magnetometer_raw[2] - mag_offset[2]) * 0.01f; + + /* Fill in gyro measurements */ + z_k[6] = raw.gyro_raw[0] * 0.00026631611f /* = gyro * (500.0f / 180.0f * pi / 32768.0f ) */; + z_k[7] = raw.gyro_raw[1] * 0.00026631611f /* = gyro * (500.0f / 180.0f * pi / 32768.0f ) */; + z_k[8] = raw.gyro_raw[2] * 0.00026631611f /* = gyro * (500.0f / 180.0f * pi / 32768.0f ) */; + + + uint64_t now = hrt_absolute_time(); + unsigned int time_elapsed = now - last_run; + last_run = now; + + if (time_elapsed > loop_interval_alarm) { + //TODO: add warning, cpu overload here + if (overloadcounter == 20) { + printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR BLACK MAGIC (%lu > %lu)\n", time_elapsed, loop_interval_alarm); + overloadcounter = 0; + } + + overloadcounter++; + } + +// now = hrt_absolute_time(); + /* filter values */ + /* + * function [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) + */ + uint64_t timing_start = hrt_absolute_time(); + attitudeKalmanfilter(dt, z_k, x_aposteriori, P_aposteriori, knownConst, Rot_matrix, x_aposteriori, P_aposteriori); + uint64_t timing_diff = hrt_absolute_time() - timing_start; + + /* print rotation matrix every 200th time */ + if (printcounter % 200 == 0) { + 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("\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++; + +// time_elapsed = hrt_absolute_time() - now; +// if (blubb == 20) +// { +// printf("Estimator: %lu\n", time_elapsed); +// blubb = 0; +// } +// blubb++; + + if (last_data > 0 && raw.timestamp - last_data > 8000) printf("sensor data missed! (%llu)\n", raw.timestamp - last_data); + +// printf("%llu -> %llu = %llu\n", last_data, raw.timestamp, raw.timestamp - last_data); + last_data = raw.timestamp; + + /* send out */ + att.timestamp = raw.timestamp; + // att.roll = euler.x; + // att.pitch = euler.y; + // att.yaw = euler.z + F_M_PI; + + // if (att.yaw > 2 * F_M_PI) { + // att.yaw -= 2 * F_M_PI; + // } + + // att.rollspeed = rates.x; + // att.pitchspeed = rates.y; + // att.yawspeed = rates.z; + + // memcpy()R + + // Broadcast + orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); + } + + loopcounter++; + } + + /* Should never reach here */ + return 0; +} + + + + + diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c new file mode 100755 index 000000000..4e275e3ee --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c @@ -0,0 +1,635 @@ +/* + * attitudeKalmanfilter.c + * + * Code generation for function 'attitudeKalmanfilter' + * + * C source code generated on: Wed Jul 11 08:38:35 2012 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "attitudeKalmanfilter.h" +#include "norm.h" +#include "eye.h" +#include "mrdivide.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ + +/* + * function [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) + */ +void attitudeKalmanfilter(real32_T dt, const real32_T z_k[9], const real32_T + x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T + knownConst[7], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T + P_aposteriori[144]) +{ + real32_T R_temp[9]; + real_T dv0[9]; + real_T dv1[9]; + int32_T i; + int32_T i0; + real32_T A_pred[144]; + real32_T x_apriori[12]; + real32_T b_A_pred[144]; + int32_T i1; + real32_T c_A_pred[144]; + static const int8_T iv0[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; + + real32_T P_apriori[144]; + real32_T b_P_apriori[108]; + static const int8_T iv1[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, + 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1 }; + + real32_T K_k[108]; + static const int8_T iv2[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; + + real32_T fv0[81]; + real32_T fv1[81]; + real32_T fv2[81]; + real32_T B; + real_T dv2[144]; + real32_T b_B; + real32_T earth_z[3]; + real32_T y[3]; + real32_T earth_x[3]; + + /* Extended Attitude Kalmanfilter */ + /* */ + /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ + /* measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]' */ + /* knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */ + /* */ + /* [x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */ + /* */ + /* Example.... */ + /* */ + /* $Author: Tobias Naegeli $ $Date: 2012 $ $Revision: 1 $ */ + /* %define the matrices */ + /* 'attitudeKalmanfilter:19' acc_ProcessNoise=knownConst(1); */ + /* 'attitudeKalmanfilter:20' mag_ProcessNoise=knownConst(2); */ + /* 'attitudeKalmanfilter:21' ratesOffset_ProcessNoise=knownConst(3); */ + /* 'attitudeKalmanfilter:22' rates_ProcessNoise=knownConst(4); */ + /* 'attitudeKalmanfilter:25' acc_MeasurementNoise=knownConst(5); */ + /* 'attitudeKalmanfilter:26' mag_MeasurementNoise=knownConst(6); */ + /* 'attitudeKalmanfilter:27' rates_MeasurementNoise=knownConst(7); */ + /* process noise covariance matrix */ + /* 'attitudeKalmanfilter:30' Q = [ eye(3)*acc_ProcessNoise, zeros(3), zeros(3), zeros(3); */ + /* 'attitudeKalmanfilter:31' zeros(3), eye(3)*mag_ProcessNoise, zeros(3), zeros(3); */ + /* 'attitudeKalmanfilter:32' zeros(3), zeros(3), eye(3)*ratesOffset_ProcessNoise, zeros(3); */ + /* 'attitudeKalmanfilter:33' zeros(3), zeros(3), zeros(3), eye(3)*rates_ProcessNoise]; */ + /* measurement noise covariance matrix */ + /* 'attitudeKalmanfilter:36' R = [ eye(3)*acc_MeasurementNoise, zeros(3), zeros(3); */ + /* 'attitudeKalmanfilter:37' zeros(3), eye(3)*mag_MeasurementNoise, zeros(3); */ + /* 'attitudeKalmanfilter:38' zeros(3), zeros(3), eye(3)*rates_MeasurementNoise]; */ + /* observation matrix */ + /* 'attitudeKalmanfilter:42' H_k=[ eye(3), zeros(3), zeros(3), zeros(3); */ + /* 'attitudeKalmanfilter:43' zeros(3), eye(3), zeros(3), zeros(3); */ + /* 'attitudeKalmanfilter:44' zeros(3), zeros(3), eye(3), eye(3)]; */ + /* compute A(t,w) */ + /* x_aposteriori_k[10,11,12] should be [p,q,r] */ + /* R_temp=[1,-r, q */ + /* r, 1, -p */ + /* -q, p, 1] */ + /* 'attitudeKalmanfilter:53' R_temp=[1,-dt*x_aposteriori_k(12),dt*x_aposteriori_k(11); */ + /* 'attitudeKalmanfilter:54' dt*x_aposteriori_k(12),1,-dt*x_aposteriori_k(10); */ + /* 'attitudeKalmanfilter:55' -dt*x_aposteriori_k(11), dt*x_aposteriori_k(10),1]; */ + R_temp[0] = 1.0F; + R_temp[3] = -dt * x_aposteriori_k[11]; + R_temp[6] = dt * x_aposteriori_k[10]; + R_temp[1] = dt * x_aposteriori_k[11]; + R_temp[4] = 1.0F; + R_temp[7] = -dt * x_aposteriori_k[9]; + R_temp[2] = -dt * x_aposteriori_k[10]; + R_temp[5] = dt * x_aposteriori_k[9]; + R_temp[8] = 1.0F; + + /* strange, should not be transposed */ + /* 'attitudeKalmanfilter:58' A_pred=[R_temp', zeros(3), zeros(3), zeros(3); */ + /* 'attitudeKalmanfilter:59' zeros(3), R_temp', zeros(3), zeros(3); */ + /* 'attitudeKalmanfilter:60' zeros(3), zeros(3), eye(3), zeros(3); */ + /* 'attitudeKalmanfilter:61' zeros(3), zeros(3), zeros(3), eye(3)]; */ + eye(dv0); + eye(dv1); + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_pred[i0 + 12 * i] = R_temp[i + 3 * i0]; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_pred[i0 + 12 * (i + 3)] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_pred[i0 + 12 * (i + 6)] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_pred[i0 + 12 * (i + 9)] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_pred[(i0 + 12 * i) + 3] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_pred[(i0 + 12 * (i + 3)) + 3] = R_temp[i + 3 * i0]; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_pred[(i0 + 12 * (i + 6)) + 3] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_pred[(i0 + 12 * (i + 9)) + 3] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_pred[(i0 + 12 * i) + 6] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_pred[(i0 + 12 * (i + 3)) + 6] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_pred[(i0 + 12 * (i + 6)) + 6] = (real32_T)dv0[i0 + 3 * i]; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_pred[(i0 + 12 * (i + 9)) + 6] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_pred[(i0 + 12 * i) + 9] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_pred[(i0 + 12 * (i + 3)) + 9] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_pred[(i0 + 12 * (i + 6)) + 9] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_pred[(i0 + 12 * (i + 9)) + 9] = (real32_T)dv1[i0 + 3 * i]; + } + } + + /* %prediction step */ + /* 'attitudeKalmanfilter:64' x_apriori=A_pred*x_aposteriori_k; */ + for (i = 0; i < 12; i++) { + x_apriori[i] = 0.0F; + for (i0 = 0; i0 < 12; i0++) { + x_apriori[i] += A_pred[i + 12 * i0] * x_aposteriori_k[i0]; + } + } + + /* linearization */ + /* 'attitudeKalmanfilter:67' acc_temp_mat=[0, dt*x_aposteriori_k(3), -dt*x_aposteriori_k(2); */ + /* 'attitudeKalmanfilter:68' -dt*x_aposteriori_k(3), 0, dt*x_aposteriori_k(1); */ + /* 'attitudeKalmanfilter:69' dt*x_aposteriori_k(2), -dt*x_aposteriori_k(1), 0]; */ + /* 'attitudeKalmanfilter:71' mag_temp_mat=[0, dt*x_aposteriori_k(6), -dt*x_aposteriori_k(5); */ + /* 'attitudeKalmanfilter:72' -dt*x_aposteriori_k(6), 0, dt*x_aposteriori_k(4); */ + /* 'attitudeKalmanfilter:73' dt*x_aposteriori_k(5), -dt*x_aposteriori_k(4), 0]; */ + /* 'attitudeKalmanfilter:75' A_lin=[R_temp', zeros(3), zeros(3), acc_temp_mat'; */ + /* 'attitudeKalmanfilter:76' zeros(3), R_temp', zeros(3), mag_temp_mat'; */ + /* 'attitudeKalmanfilter:77' zeros(3), zeros(3), eye(3), zeros(3); */ + /* 'attitudeKalmanfilter:78' zeros(3), zeros(3), zeros(3), eye(3)]; */ + eye(dv0); + eye(dv1); + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_pred[i0 + 12 * i] = R_temp[i + 3 * i0]; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_pred[i0 + 12 * (i + 3)] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_pred[i0 + 12 * (i + 6)] = 0.0F; + } + } + + A_pred[108] = 0.0F; + A_pred[109] = dt * x_aposteriori_k[2]; + A_pred[110] = -dt * x_aposteriori_k[1]; + A_pred[120] = -dt * x_aposteriori_k[2]; + A_pred[121] = 0.0F; + A_pred[122] = dt * x_aposteriori_k[0]; + A_pred[132] = dt * x_aposteriori_k[1]; + A_pred[133] = -dt * x_aposteriori_k[0]; + A_pred[134] = 0.0F; + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_pred[(i0 + 12 * i) + 3] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_pred[(i0 + 12 * (i + 3)) + 3] = R_temp[i + 3 * i0]; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_pred[(i0 + 12 * (i + 6)) + 3] = 0.0F; + } + } + + A_pred[111] = 0.0F; + A_pred[112] = dt * x_aposteriori_k[5]; + A_pred[113] = -dt * x_aposteriori_k[4]; + A_pred[123] = -dt * x_aposteriori_k[5]; + A_pred[124] = 0.0F; + A_pred[125] = dt * x_aposteriori_k[3]; + A_pred[135] = dt * x_aposteriori_k[4]; + A_pred[136] = -dt * x_aposteriori_k[3]; + A_pred[137] = 0.0F; + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_pred[(i0 + 12 * i) + 6] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_pred[(i0 + 12 * (i + 3)) + 6] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_pred[(i0 + 12 * (i + 6)) + 6] = (real32_T)dv0[i0 + 3 * i]; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_pred[(i0 + 12 * (i + 9)) + 6] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_pred[(i0 + 12 * i) + 9] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_pred[(i0 + 12 * (i + 3)) + 9] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_pred[(i0 + 12 * (i + 6)) + 9] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + A_pred[(i0 + 12 * (i + 9)) + 9] = (real32_T)dv1[i0 + 3 * i]; + } + } + + /* 'attitudeKalmanfilter:81' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */ + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 12; i0++) { + b_A_pred[i + 12 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + b_A_pred[i + 12 * i0] += A_pred[i + 12 * i1] * P_aposteriori_k[i1 + 12 * + i0]; + } + } + + for (i0 = 0; i0 < 12; i0++) { + c_A_pred[i + 12 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + c_A_pred[i + 12 * i0] += b_A_pred[i + 12 * i1] * A_pred[i0 + 12 * i1]; + } + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + b_A_pred[i0 + 12 * i] = (real32_T)iv0[i0 + 3 * i] * knownConst[0]; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + b_A_pred[i0 + 12 * (i + 3)] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + b_A_pred[i0 + 12 * (i + 6)] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + b_A_pred[i0 + 12 * (i + 9)] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + b_A_pred[(i0 + 12 * i) + 3] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + b_A_pred[(i0 + 12 * (i + 3)) + 3] = (real32_T)iv0[i0 + 3 * i] * + knownConst[1]; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + b_A_pred[(i0 + 12 * (i + 6)) + 3] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + b_A_pred[(i0 + 12 * (i + 9)) + 3] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + b_A_pred[(i0 + 12 * i) + 6] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + b_A_pred[(i0 + 12 * (i + 3)) + 6] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + b_A_pred[(i0 + 12 * (i + 6)) + 6] = (real32_T)iv0[i0 + 3 * i] * + knownConst[2]; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + b_A_pred[(i0 + 12 * (i + 9)) + 6] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + b_A_pred[(i0 + 12 * i) + 9] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + b_A_pred[(i0 + 12 * (i + 3)) + 9] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + b_A_pred[(i0 + 12 * (i + 6)) + 9] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + b_A_pred[(i0 + 12 * (i + 9)) + 9] = (real32_T)iv0[i0 + 3 * i] * + knownConst[3]; + } + } + + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 12; i0++) { + P_apriori[i0 + 12 * i] = c_A_pred[i0 + 12 * i] + b_A_pred[i0 + 12 * i]; + } + } + + /* %update step */ + /* 'attitudeKalmanfilter:86' y_k=z_k-H_k*x_apriori; */ + /* 'attitudeKalmanfilter:87' S_k=H_k*P_apriori*H_k'+R; */ + /* 'attitudeKalmanfilter:88' K_k=(P_apriori*H_k'/(S_k)); */ + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 9; i0++) { + b_P_apriori[i + 12 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + b_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)iv1[i1 + + 12 * i0]; + } + } + } + + for (i = 0; i < 9; i++) { + for (i0 = 0; i0 < 12; i0++) { + K_k[i + 9 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + K_k[i + 9 * i0] += (real32_T)iv2[i + 9 * i1] * P_apriori[i1 + 12 * i0]; + } + } + + for (i0 = 0; i0 < 9; i0++) { + fv0[i + 9 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + fv0[i + 9 * i0] += K_k[i + 9 * i1] * (real32_T)iv1[i1 + 12 * i0]; + } + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + fv1[i0 + 9 * i] = (real32_T)iv0[i0 + 3 * i] * knownConst[4]; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + fv1[i0 + 9 * (i + 3)] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + fv1[i0 + 9 * (i + 6)] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + fv1[(i0 + 9 * i) + 3] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + fv1[(i0 + 9 * (i + 3)) + 3] = (real32_T)iv0[i0 + 3 * i] * knownConst[5]; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + fv1[(i0 + 9 * (i + 6)) + 3] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + fv1[(i0 + 9 * i) + 6] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + fv1[(i0 + 9 * (i + 3)) + 6] = 0.0F; + } + } + + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + fv1[(i0 + 9 * (i + 6)) + 6] = (real32_T)iv0[i0 + 3 * i] * knownConst[6]; + } + } + + for (i = 0; i < 9; i++) { + for (i0 = 0; i0 < 9; i0++) { + fv2[i0 + 9 * i] = fv0[i0 + 9 * i] + fv1[i0 + 9 * i]; + } + } + + mrdivide(b_P_apriori, fv2, K_k); + + /* 'attitudeKalmanfilter:91' x_aposteriori=x_apriori+K_k*y_k; */ + for (i = 0; i < 9; i++) { + B = 0.0F; + for (i0 = 0; i0 < 12; i0++) { + B += (real32_T)iv2[i + 9 * i0] * x_apriori[i0]; + } + + R_temp[i] = z_k[i] - B; + } + + for (i = 0; i < 12; i++) { + B = 0.0F; + for (i0 = 0; i0 < 9; i0++) { + B += K_k[i + 12 * i0] * R_temp[i0]; + } + + x_aposteriori[i] = x_apriori[i] + B; + } + + /* 'attitudeKalmanfilter:92' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */ + b_eye(dv2); + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 12; i0++) { + B = 0.0F; + for (i1 = 0; i1 < 9; i1++) { + B += K_k[i + 12 * i1] * (real32_T)iv2[i1 + 9 * i0]; + } + + A_pred[i + 12 * i0] = (real32_T)dv2[i + 12 * i0] - B; + } + } + + for (i = 0; i < 12; i++) { + for (i0 = 0; i0 < 12; i0++) { + P_aposteriori[i + 12 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + P_aposteriori[i + 12 * i0] += A_pred[i + 12 * i1] * P_apriori[i1 + 12 * + i0]; + } + } + } + + /* %Rotation matrix generation */ + /* 'attitudeKalmanfilter:97' earth_z=x_aposteriori(1:3)/norm(x_aposteriori(1:3)); */ + B = norm(*(real32_T (*)[3])&x_aposteriori[0]); + + /* 'attitudeKalmanfilter:98' earth_x=cross(earth_z,x_aposteriori(4:6)/norm(x_aposteriori(4:6))); */ + b_B = norm(*(real32_T (*)[3])&x_aposteriori[3]); + for (i = 0; i < 3; i++) { + earth_z[i] = x_aposteriori[i] / B; + y[i] = x_aposteriori[i + 3] / b_B; + } + + earth_x[0] = earth_z[1] * y[2] - earth_z[2] * y[1]; + earth_x[1] = earth_z[2] * y[0] - earth_z[0] * y[2]; + earth_x[2] = earth_z[0] * y[1] - earth_z[1] * y[0]; + + /* 'attitudeKalmanfilter:99' earth_y=cross(earth_x,earth_z); */ + /* 'attitudeKalmanfilter:101' Rot_matrix=[earth_x,earth_y,earth_z]; */ + y[0] = earth_x[1] * earth_z[2] - earth_x[2] * earth_z[1]; + y[1] = earth_x[2] * earth_z[0] - earth_x[0] * earth_z[2]; + y[2] = earth_x[0] * earth_z[1] - earth_x[1] * earth_z[0]; + for (i = 0; i < 3; i++) { + Rot_matrix[i] = earth_x[i]; + Rot_matrix[3 + i] = y[i]; + Rot_matrix[6 + i] = earth_z[i]; + } +} + +/* End of code generation (attitudeKalmanfilter.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h new file mode 100755 index 000000000..7aa3d048b --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h @@ -0,0 +1,32 @@ +/* + * attitudeKalmanfilter.h + * + * Code generation for function 'attitudeKalmanfilter' + * + * C source code generated on: Wed Jul 11 08:38:35 2012 + * + */ + +#ifndef __ATTITUDEKALMANFILTER_H__ +#define __ATTITUDEKALMANFILTER_H__ +/* Include files */ +#include +#include +#include +#include + +#include "rtwtypes.h" +#include "attitudeKalmanfilter_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void attitudeKalmanfilter(real32_T dt, const real32_T z_k[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T knownConst[7], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]); +#endif +/* End of code generation (attitudeKalmanfilter.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c new file mode 100755 index 000000000..2800191c3 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c @@ -0,0 +1,31 @@ +/* + * attitudeKalmanfilter_initialize.c + * + * Code generation for function 'attitudeKalmanfilter_initialize' + * + * C source code generated on: Wed Jul 11 08:38:35 2012 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "attitudeKalmanfilter.h" +#include "attitudeKalmanfilter_initialize.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ +void attitudeKalmanfilter_initialize(void) +{ + rt_InitInfAndNaN(8U); +} + +/* End of code generation (attitudeKalmanfilter_initialize.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h new file mode 100755 index 000000000..d2d97bb3b --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h @@ -0,0 +1,32 @@ +/* + * attitudeKalmanfilter_initialize.h + * + * Code generation for function 'attitudeKalmanfilter_initialize' + * + * C source code generated on: Wed Jul 11 08:38:35 2012 + * + */ + +#ifndef __ATTITUDEKALMANFILTER_INITIALIZE_H__ +#define __ATTITUDEKALMANFILTER_INITIALIZE_H__ +/* Include files */ +#include +#include +#include +#include + +#include "rtwtypes.h" +#include "attitudeKalmanfilter_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void attitudeKalmanfilter_initialize(void); +#endif +/* End of code generation (attitudeKalmanfilter_initialize.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_ref.rsp b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_ref.rsp new file mode 100755 index 000000000..e69de29bb diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.bat b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.bat new file mode 100755 index 000000000..76fb78ca7 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.bat @@ -0,0 +1,11 @@ +@echo off +call "%VS100COMNTOOLS%..\..\VC\vcvarsall.bat" AMD64 + +cd . +nmake -f attitudeKalmanfilter_rtw.mk GENERATE_REPORT=1 ADD_MDL_NAME_TO_GLOBALS=0 +@if errorlevel 1 goto error_exit +exit /B 0 + +:error_exit +echo The make command returned an error of %errorlevel% +An_error_occurred_during_the_call_to_make diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.mk b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.mk new file mode 100755 index 000000000..b2123704b --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.mk @@ -0,0 +1,328 @@ +# Copyright 1994-2010 The MathWorks, Inc. +# +# File : xrt_vcx64.tmf $Revision: 1.1.6.1 $ +# +# Abstract: +# Code generation template makefile for building a Windows-based, +# stand-alone real-time version of MATLAB-code using generated C/C++ +# code and the +# Microsoft Visual C/C++ compiler version 8, 9, 10 for x64 +# +# Note that this template is automatically customized by the +# code generation build procedure to create ".mk" +# +# The following defines can be used to modify the behavior of the +# build: +# +# OPT_OPTS - Optimization option. See DEFAULT_OPT_OPTS in +# vctools.mak for default. +# OPTS - User specific options. +# CPP_OPTS - C++ compiler options. +# USER_SRCS - Additional user sources, such as files needed by +# S-functions. +# USER_INCLUDES - Additional include paths +# (i.e. USER_INCLUDES="-Iwhere-ever -Iwhere-ever2") +# +# To enable debugging: +# set OPT_OPTS=-Zi (may vary with compiler version, see compiler doc) +# modify tmf LDFLAGS to include /DEBUG +# + +#------------------------ Macros read by make_rtw ----------------------------- +# +# The following macros are read by the code generation build procedure: +# +# MAKECMD - This is the command used to invoke the make utility +# HOST - What platform this template makefile is targeted for +# (i.e. PC or UNIX) +# BUILD - Invoke make from the code generation build procedure +# (yes/no)? +# SYS_TARGET_FILE - Name of system target file. + +MAKECMD = nmake +HOST = PC +BUILD = yes +SYS_TARGET_FILE = ert.tlc +BUILD_SUCCESS = ^#^#^# Created +COMPILER_TOOL_CHAIN = vcx64 + +#---------------------- Tokens expanded by make_rtw --------------------------- +# +# The following tokens, when wrapped with "|>" and "<|" are expanded by the +# code generation build procedure. +# +# MODEL_NAME - Name of the Simulink block diagram +# MODEL_MODULES - Any additional generated source modules +# MAKEFILE_NAME - Name of makefile created from template makefile .mk +# MATLAB_ROOT - Path to where MATLAB is installed. +# MATLAB_BIN - Path to MATLAB executable. +# S_FUNCTIONS - List of S-functions. +# S_FUNCTIONS_LIB - List of S-functions libraries to link. +# SOLVER - Solver source file name +# NUMST - Number of sample times +# TID01EQ - yes (1) or no (0): Are sampling rates of continuous task +# (tid=0) and 1st discrete task equal. +# NCSTATES - Number of continuous states +# BUILDARGS - Options passed in at the command line. +# MULTITASKING - yes (1) or no (0): Is solver mode multitasking +# EXT_MODE - yes (1) or no (0): Build for external mode +# TMW_EXTMODE_TESTING - yes (1) or no (0): Build ext_test.c for external mode +# testing. +# EXTMODE_TRANSPORT - Index of transport mechanism (e.g. tcpip, serial) for extmode +# EXTMODE_STATIC - yes (1) or no (0): Use static instead of dynamic mem alloc. +# EXTMODE_STATIC_SIZE - Size of static memory allocation buffer. + +MODEL = attitudeKalmanfilter +MODULES = attitudeKalmanfilter.c eye.c mrdivide.c norm.c attitudeKalmanfilter_initialize.c attitudeKalmanfilter_terminate.c rt_nonfinite.c rtGetNaN.c rtGetInf.c +MAKEFILE = attitudeKalmanfilter_rtw.mk +MATLAB_ROOT = C:\Program Files\MATLAB\R2011a +ALT_MATLAB_ROOT = C:\PROGRA~1\MATLAB\R2011a +MATLAB_BIN = C:\Program Files\MATLAB\R2011a\bin +ALT_MATLAB_BIN = C:\PROGRA~1\MATLAB\R2011a\bin +S_FUNCTIONS = +S_FUNCTIONS_LIB = +SOLVER = +NUMST = 1 +TID01EQ = 0 +NCSTATES = 0 +BUILDARGS = GENERATE_REPORT=1 ADD_MDL_NAME_TO_GLOBALS=0 +MULTITASKING = 0 +EXT_MODE = 0 +TMW_EXTMODE_TESTING = 0 +EXTMODE_TRANSPORT = 0 +EXTMODE_STATIC = 0 +EXTMODE_STATIC_SIZE = 1000000 + +MODELREFS = +SHARED_SRC = +SHARED_SRC_DIR = +SHARED_BIN_DIR = +SHARED_LIB = +TARGET_LANG_EXT = c +OPTIMIZATION_FLAGS = /O2 /Oy- +ADDITIONAL_LDFLAGS = + +#--------------------------- Model and reference models ----------------------- +MODELLIB = attitudeKalmanfilter.lib +MODELREF_LINK_LIBS = +MODELREF_LINK_RSPFILE = attitudeKalmanfilter_ref.rsp +MODELREF_INC_PATH = +RELATIVE_PATH_TO_ANCHOR = F:\CODEGE~1 +MODELREF_TARGET_TYPE = RTW + +!if "$(MATLAB_ROOT)" != "$(ALT_MATLAB_ROOT)" +MATLAB_ROOT = $(ALT_MATLAB_ROOT) +!endif +!if "$(MATLAB_BIN)" != "$(ALT_MATLAB_BIN)" +MATLAB_BIN = $(ALT_MATLAB_BIN) +!endif + +#--------------------------- Tool Specifications ------------------------------ + +CPU = AMD64 +!include $(MATLAB_ROOT)\rtw\c\tools\vctools.mak +APPVER = 5.02 + +PERL = $(MATLAB_ROOT)\sys\perl\win32\bin\perl +#------------------------------ Include/Lib Path ------------------------------ + +MATLAB_INCLUDES = $(MATLAB_ROOT)\simulink\include +MATLAB_INCLUDES = $(MATLAB_INCLUDES);$(MATLAB_ROOT)\extern\include +MATLAB_INCLUDES = $(MATLAB_INCLUDES);$(MATLAB_ROOT)\rtw\c\src +MATLAB_INCLUDES = $(MATLAB_INCLUDES);$(MATLAB_ROOT)\rtw\c\src\ext_mode\common + +# Additional file include paths + +MATLAB_INCLUDES = $(MATLAB_INCLUDES);F:\codegenerationMatlabAttFilter\codegen\lib\attitudeKalmanfilter +MATLAB_INCLUDES = $(MATLAB_INCLUDES);F:\codegenerationMatlabAttFilter + +INCLUDE = .;$(RELATIVE_PATH_TO_ANCHOR);$(MATLAB_INCLUDES);$(INCLUDE);$(MODELREF_INC_PATH) + +!if "$(SHARED_SRC_DIR)" != "" +INCLUDE = $(INCLUDE);$(SHARED_SRC_DIR) +!endif + +#------------------------ External mode --------------------------------------- +# Uncomment -DVERBOSE to have information printed to stdout +# To add a new transport layer, see the comments in +# /toolbox/simulink/simulink/extmode_transports.m +!if $(EXT_MODE) == 1 +EXT_CC_OPTS = -DEXT_MODE # -DVERBOSE +!if $(EXTMODE_TRANSPORT) == 0 #tcpip +EXT_SRC = updown.c rtiostream_interface.c rtiostream_tcpip.c +EXT_LIB = wsock32.lib +!endif +!if $(EXTMODE_TRANSPORT) == 1 #serial_win32 +EXT_SRC = ext_svr.c updown.c ext_work.c ext_svr_serial_transport.c +EXT_SRC = $(EXT_SRC) ext_serial_pkt.c ext_serial_win32_port.c +EXT_LIB = +!endif +!if $(TMW_EXTMODE_TESTING) == 1 +EXT_SRC = $(EXT_SRC) ext_test.c +EXT_CC_OPTS = $(EXT_CC_OPTS) -DTMW_EXTMODE_TESTING +!endif +!if $(EXTMODE_STATIC) == 1 +EXT_SRC = $(EXT_SRC) mem_mgr.c +EXT_CC_OPTS = $(EXT_CC_OPTS) -DEXTMODE_STATIC -DEXTMODE_STATIC_SIZE=$(EXTMODE_STATIC_SIZE) +!endif +!else +EXT_SRC = +EXT_CC_OPTS = +EXT_LIB = +!endif + +#------------------------ rtModel ---------------------------------------------- + +RTM_CC_OPTS = -DUSE_RTMODEL + +#----------------- Compiler and Linker Options -------------------------------- + +# Optimization Options +# Set OPT_OPTS=-Zi for debugging (may depend on compiler version) +OPT_OPTS = $(DEFAULT_OPT_OPTS) + +# General User Options +OPTS = + +!if "$(OPTIMIZATION_FLAGS)" != "" +CC_OPTS = $(OPTS) $(EXT_CC_OPTS) $(RTM_CC_OPTS) $(OPTIMIZATION_FLAGS) +!else +CC_OPTS = $(OPT_OPTS) $(OPTS) $(EXT_CC_OPTS) $(RTM_CC_OPTS) +!endif + +CPP_REQ_DEFINES = -DMODEL=$(MODEL) -DRT -DNUMST=$(NUMST) \ + -DTID01EQ=$(TID01EQ) -DNCSTATES=$(NCSTATES) \ + -DMT=$(MULTITASKING) -DHAVESTDIO + +# Uncomment this line to move warning level to W4 +# cflags = $(cflags:W3=W4) +CFLAGS = $(CC_OPTS) $(CPP_REQ_DEFINES) $(USER_INCLUDES) \ + $(cflags) $(cvarsmt) /wd4996 +CPPFLAGS = $(CPP_OPTS) $(CC_OPTS) $(CPP_REQ_DEFINES) $(USER_INCLUDES) \ + $(cflags) $(cvarsmt) /wd4996 /EHsc- +LDFLAGS = $(ldebug) $(conflags) $(EXT_LIB) $(conlibs) libcpmt.lib $(ADDITIONAL_LDFLAGS) + +# libcpmt.lib is the multi-threaded, static lib version of the C++ standard lib + +#----------------------------- Source Files ----------------------------------- + + +#Standalone executable +!if "$(MODELREF_TARGET_TYPE)" == "NONE" +PRODUCT = $(RELATIVE_PATH_TO_ANCHOR)\$(MODEL).exe +REQ_SRCS = $(MODULES) $(EXT_SRC) + +#Model Reference Target +!else +PRODUCT = $(MODELLIB) +REQ_SRCS = $(MODULES) $(EXT_SRC) +!endif + +USER_SRCS = + +SRCS = $(REQ_SRCS) $(USER_SRCS) $(S_FUNCTIONS) +OBJS_CPP_UPPER = $(SRCS:.CPP=.obj) +OBJS_CPP_LOWER = $(OBJS_CPP_UPPER:.cpp=.obj) +OBJS_C_UPPER = $(OBJS_CPP_LOWER:.C=.obj) +OBJS = $(OBJS_C_UPPER:.c=.obj) +SHARED_OBJS = $(SHARED_SRC:.c=.obj) +# ------------------------- Additional Libraries ------------------------------ + +LIBS = + + +LIBS = $(LIBS) + +# ---------------------------- Linker Script ---------------------------------- + +CMD_FILE = $(MODEL).lnk +GEN_LNK_SCRIPT = $(MATLAB_ROOT)\rtw\c\tools\mkvc_lnk.pl + +#--------------------------------- Rules -------------------------------------- +all: set_environment_variables $(PRODUCT) + +!if "$(MODELREF_TARGET_TYPE)" == "NONE" +#--- Stand-alone model --- +$(PRODUCT) : $(OBJS) $(SHARED_LIB) $(LIBS) $(MODELREF_LINK_LIBS) + @cmd /C "echo ### Linking ..." + $(PERL) $(GEN_LNK_SCRIPT) $(CMD_FILE) $(OBJS) + $(LD) $(LDFLAGS) $(S_FUNCTIONS_LIB) $(SHARED_LIB) $(LIBS) $(MAT_LIBS) @$(CMD_FILE) @$(MODELREF_LINK_RSPFILE) -out:$@ + @del $(CMD_FILE) + @cmd /C "echo $(BUILD_SUCCESS) executable $(MODEL).exe" +!else +#--- Model reference code generation Target --- +$(PRODUCT) : $(OBJS) $(SHARED_LIB) + @cmd /C "echo ### Linking ..." + $(PERL) $(GEN_LNK_SCRIPT) $(CMD_FILE) $(OBJS) + $(LD) -lib /OUT:$(PRODUCT) @$(CMD_FILE) $(S_FUNCTIONS_LIB) + @cmd /C "echo $(BUILD_SUCCESS) static library $(MODELLIB)" +!endif + +{$(MATLAB_ROOT)\rtw\c\src}.c.obj : + @cmd /C "echo ### Compiling $<" + $(CC) $(CFLAGS) $< + +{$(MATLAB_ROOT)\rtw\c\src\ext_mode\common}.c.obj : + @cmd /C "echo ### Compiling $<" + $(CC) $(CFLAGS) $< + +{$(MATLAB_ROOT)\rtw\c\src\rtiostream\rtiostreamtcpip}.c.obj : + @cmd /C "echo ### Compiling $<" + $(CC) $(CFLAGS) $< + +{$(MATLAB_ROOT)\rtw\c\src\ext_mode\serial}.c.obj : + @cmd /C "echo ### Compiling $<" + $(CC) $(CFLAGS) $< + +{$(MATLAB_ROOT)\rtw\c\src\ext_mode\custom}.c.obj : + @cmd /C "echo ### Compiling $<" + $(CC) $(CFLAGS) $< + +# Additional sources + + + + + +# Put these rule last, otherwise nmake will check toolboxes first + +{$(RELATIVE_PATH_TO_ANCHOR)}.c.obj : + @cmd /C "echo ### Compiling $<" + $(CC) $(CFLAGS) $< + +{$(RELATIVE_PATH_TO_ANCHOR)}.cpp.obj : + @cmd /C "echo ### Compiling $<" + $(CC) $(CPPFLAGS) $< + +.c.obj : + @cmd /C "echo ### Compiling $<" + $(CC) $(CFLAGS) $< + +.cpp.obj : + @cmd /C "echo ### Compiling $<" + $(CC) $(CPPFLAGS) $< + +!if "$(SHARED_LIB)" != "" +$(SHARED_LIB) : $(SHARED_SRC) + @cmd /C "echo ### Creating $@" + @$(CC) $(CFLAGS) -Fo$(SHARED_BIN_DIR)\ @<< +$? +<< + @$(LIBCMD) /nologo /out:$@ $(SHARED_OBJS) + @cmd /C "echo ### $@ Created" +!endif + + +set_environment_variables: + @set INCLUDE=$(INCLUDE) + @set LIB=$(LIB) + +# Libraries: + + + + + +#----------------------------- Dependencies ----------------------------------- + +$(OBJS) : $(MAKEFILE) rtw_proj.tmw diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c new file mode 100755 index 000000000..eab8c7d6e --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c @@ -0,0 +1,31 @@ +/* + * attitudeKalmanfilter_terminate.c + * + * Code generation for function 'attitudeKalmanfilter_terminate' + * + * C source code generated on: Wed Jul 11 08:38:35 2012 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "attitudeKalmanfilter.h" +#include "attitudeKalmanfilter_terminate.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ +void attitudeKalmanfilter_terminate(void) +{ + /* (no terminate code required) */ +} + +/* End of code generation (attitudeKalmanfilter_terminate.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h new file mode 100755 index 000000000..fafd866e4 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h @@ -0,0 +1,32 @@ +/* + * attitudeKalmanfilter_terminate.h + * + * Code generation for function 'attitudeKalmanfilter_terminate' + * + * C source code generated on: Wed Jul 11 08:38:35 2012 + * + */ + +#ifndef __ATTITUDEKALMANFILTER_TERMINATE_H__ +#define __ATTITUDEKALMANFILTER_TERMINATE_H__ +/* Include files */ +#include +#include +#include +#include + +#include "rtwtypes.h" +#include "attitudeKalmanfilter_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void attitudeKalmanfilter_terminate(void); +#endif +/* End of code generation (attitudeKalmanfilter_terminate.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h new file mode 100755 index 000000000..05f4664cd --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h @@ -0,0 +1,16 @@ +/* + * attitudeKalmanfilter_types.h + * + * Code generation for function 'attitudeKalmanfilter' + * + * C source code generated on: Wed Jul 11 08:38:35 2012 + * + */ + +#ifndef __ATTITUDEKALMANFILTER_TYPES_H__ +#define __ATTITUDEKALMANFILTER_TYPES_H__ + +/* Type Definitions */ + +#endif +/* End of code generation (attitudeKalmanfilter_types.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/buildInfo.mat b/apps/attitude_estimator_ekf/codegen/buildInfo.mat new file mode 100755 index 000000000..b811d0039 Binary files /dev/null and b/apps/attitude_estimator_ekf/codegen/buildInfo.mat differ diff --git a/apps/attitude_estimator_ekf/codegen/eye.c b/apps/attitude_estimator_ekf/codegen/eye.c new file mode 100755 index 000000000..e4655839c --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/eye.c @@ -0,0 +1,51 @@ +/* + * eye.c + * + * Code generation for function 'eye' + * + * C source code generated on: Wed Jul 11 08:38:35 2012 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "attitudeKalmanfilter.h" +#include "eye.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ + +/* + * + */ +void b_eye(real_T I[144]) +{ + int32_T i; + memset((void *)&I[0], 0, 144U * sizeof(real_T)); + for (i = 0; i < 12; i++) { + I[i + 12 * i] = 1.0; + } +} + +/* + * + */ +void eye(real_T I[9]) +{ + int32_T i; + memset((void *)&I[0], 0, 9U * sizeof(real_T)); + for (i = 0; i < 3; i++) { + I[i + 3 * i] = 1.0; + } +} + +/* End of code generation (eye.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/eye.h b/apps/attitude_estimator_ekf/codegen/eye.h new file mode 100755 index 000000000..e8881747f --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/eye.h @@ -0,0 +1,33 @@ +/* + * eye.h + * + * Code generation for function 'eye' + * + * C source code generated on: Wed Jul 11 08:38:35 2012 + * + */ + +#ifndef __EYE_H__ +#define __EYE_H__ +/* Include files */ +#include +#include +#include +#include + +#include "rtwtypes.h" +#include "attitudeKalmanfilter_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void b_eye(real_T I[144]); +extern void eye(real_T I[9]); +#endif +/* End of code generation (eye.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.c b/apps/attitude_estimator_ekf/codegen/mrdivide.c new file mode 100755 index 000000000..cb81b5328 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/mrdivide.c @@ -0,0 +1,158 @@ +/* + * mrdivide.c + * + * Code generation for function 'mrdivide' + * + * C source code generated on: Wed Jul 11 08:38:35 2012 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "attitudeKalmanfilter.h" +#include "mrdivide.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ + +/* + * + */ +void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]) +{ + int32_T jy; + int32_T iy; + real32_T b_A[81]; + int8_T ipiv[9]; + int32_T j; + int32_T mmj; + int32_T jj; + int32_T jp1j; + int32_T c; + int32_T ix; + real32_T temp; + int32_T k; + real32_T s; + int32_T loop_ub; + real32_T Y[108]; + for (jy = 0; jy < 9; jy++) { + for (iy = 0; iy < 9; iy++) { + b_A[iy + 9 * jy] = B[jy + 9 * iy]; + } + + ipiv[jy] = (int8_T)(1 + jy); + } + + for (j = 0; j < 8; j++) { + mmj = -j; + jj = j * 10; + jp1j = jj + 1; + c = mmj + 9; + jy = 0; + ix = jj; + temp = fabsf(b_A[jj]); + for (k = 2; k <= c; k++) { + ix++; + s = fabsf(b_A[ix]); + if (s > temp) { + jy = k - 1; + temp = s; + } + } + + if ((real_T)b_A[jj + jy] != 0.0) { + if (jy != 0) { + ipiv[j] = (int8_T)((j + jy) + 1); + ix = j; + iy = j + jy; + for (k = 0; k < 9; k++) { + temp = b_A[ix]; + b_A[ix] = b_A[iy]; + b_A[iy] = temp; + ix += 9; + iy += 9; + } + } + + loop_ub = (jp1j + mmj) + 8; + for (iy = jp1j; iy + 1 <= loop_ub; iy++) { + b_A[iy] /= b_A[jj]; + } + } + + c = 8 - j; + jy = jj + 9; + for (iy = 1; iy <= c; iy++) { + if ((real_T)b_A[jy] != 0.0) { + temp = b_A[jy] * -1.0F; + ix = jp1j; + loop_ub = (mmj + jj) + 18; + for (k = 10 + jj; k + 1 <= loop_ub; k++) { + b_A[k] += b_A[ix] * temp; + ix++; + } + } + + jy += 9; + jj += 9; + } + } + + for (jy = 0; jy < 12; jy++) { + for (iy = 0; iy < 9; iy++) { + Y[iy + 9 * jy] = A[jy + 12 * iy]; + } + } + + for (iy = 0; iy < 9; iy++) { + if (ipiv[iy] != iy + 1) { + for (j = 0; j < 12; j++) { + temp = Y[iy + 9 * j]; + Y[iy + 9 * j] = Y[(ipiv[iy] + 9 * j) - 1]; + Y[(ipiv[iy] + 9 * j) - 1] = temp; + } + } + } + + for (j = 0; j < 12; j++) { + c = 9 * j; + for (k = 0; k < 9; k++) { + jy = 9 * k; + if ((real_T)Y[k + c] != 0.0) { + for (iy = k + 2; iy < 10; iy++) { + Y[(iy + c) - 1] -= Y[k + c] * b_A[(iy + jy) - 1]; + } + } + } + } + + for (j = 0; j < 12; j++) { + c = 9 * j; + for (k = 8; k > -1; k += -1) { + jy = 9 * k; + if ((real_T)Y[k + c] != 0.0) { + Y[k + c] /= b_A[k + jy]; + for (iy = 0; iy + 1 <= k; iy++) { + Y[iy + c] -= Y[k + c] * b_A[iy + jy]; + } + } + } + } + + for (jy = 0; jy < 9; jy++) { + for (iy = 0; iy < 12; iy++) { + y[iy + 12 * jy] = Y[jy + 9 * iy]; + } + } +} + +/* End of code generation (mrdivide.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.h b/apps/attitude_estimator_ekf/codegen/mrdivide.h new file mode 100755 index 000000000..e81bfffc0 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/mrdivide.h @@ -0,0 +1,32 @@ +/* + * mrdivide.h + * + * Code generation for function 'mrdivide' + * + * C source code generated on: Wed Jul 11 08:38:35 2012 + * + */ + +#ifndef __MRDIVIDE_H__ +#define __MRDIVIDE_H__ +/* Include files */ +#include +#include +#include +#include + +#include "rtwtypes.h" +#include "attitudeKalmanfilter_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]); +#endif +/* End of code generation (mrdivide.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/norm.c b/apps/attitude_estimator_ekf/codegen/norm.c new file mode 100755 index 000000000..1fbde052b --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/norm.c @@ -0,0 +1,62 @@ +/* + * norm.c + * + * Code generation for function 'norm' + * + * C source code generated on: Wed Jul 11 08:38:35 2012 + * + */ + +/* Include files */ +#include "rt_nonfinite.h" +#include "attitudeKalmanfilter.h" +#include "norm.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ + +/* Function Definitions */ + +/* + * + */ +real32_T norm(const real32_T x[3]) +{ + real32_T y; + real32_T scale; + boolean_T firstNonZero; + int32_T k; + real32_T absxk; + real32_T t; + y = 0.0F; + scale = 0.0F; + firstNonZero = TRUE; + for (k = 0; k < 3; k++) { + if (x[k] != 0.0F) { + absxk = fabsf(x[k]); + if (firstNonZero) { + scale = absxk; + y = 1.0F; + firstNonZero = FALSE; + } else if (scale < absxk) { + t = scale / absxk; + y = 1.0F + y * t * t; + scale = absxk; + } else { + t = absxk / scale; + y += t * t; + } + } + } + + return scale * sqrtf(y); +} + +/* End of code generation (norm.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/norm.h b/apps/attitude_estimator_ekf/codegen/norm.h new file mode 100755 index 000000000..b0c7fe430 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/norm.h @@ -0,0 +1,32 @@ +/* + * norm.h + * + * Code generation for function 'norm' + * + * C source code generated on: Wed Jul 11 08:38:35 2012 + * + */ + +#ifndef __NORM_H__ +#define __NORM_H__ +/* Include files */ +#include +#include +#include +#include + +#include "rtwtypes.h" +#include "attitudeKalmanfilter_types.h" + +/* Type Definitions */ + +/* Named Constants */ + +/* Variable Declarations */ + +/* Variable Definitions */ + +/* Function Declarations */ +extern real32_T norm(const real32_T x[3]); +#endif +/* End of code generation (norm.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.c b/apps/attitude_estimator_ekf/codegen/rtGetInf.c new file mode 100755 index 000000000..3cef17684 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/rtGetInf.c @@ -0,0 +1,139 @@ +/* + * rtGetInf.c + * + * Code generation for function 'attitudeKalmanfilter' + * + * C source code generated on: Wed Jul 11 08:38:35 2012 + * + */ + +/* + * Abstract: + * MATLAB for code generation function to initialize non-finite, Inf and MinusInf + */ +#include "rtGetInf.h" +#define NumBitsPerChar 8U + +/* Function: rtGetInf ================================================== + * Abstract: + * Initialize rtInf needed by the generated code. + * Inf is initialized as non-signaling. Assumes IEEE. + */ +real_T rtGetInf(void) +{ + size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); + real_T inf = 0.0; + if (bitsPerReal == 32U) { + inf = rtGetInfF(); + } else { + uint16_T one = 1U; + enum { + LittleEndian, + BigEndian + } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; + switch (machByteOrder) { + case LittleEndian: + { + union { + LittleEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0x7FF00000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + inf = tmpVal.fltVal; + break; + } + + case BigEndian: + { + union { + BigEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0x7FF00000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + inf = tmpVal.fltVal; + break; + } + } + } + + return inf; +} + +/* Function: rtGetInfF ================================================== + * Abstract: + * Initialize rtInfF needed by the generated code. + * Inf is initialized as non-signaling. Assumes IEEE. + */ +real32_T rtGetInfF(void) +{ + IEEESingle infF; + infF.wordL.wordLuint = 0x7F800000U; + return infF.wordL.wordLreal; +} + +/* Function: rtGetMinusInf ================================================== + * Abstract: + * Initialize rtMinusInf needed by the generated code. + * Inf is initialized as non-signaling. Assumes IEEE. + */ +real_T rtGetMinusInf(void) +{ + size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); + real_T minf = 0.0; + if (bitsPerReal == 32U) { + minf = rtGetMinusInfF(); + } else { + uint16_T one = 1U; + enum { + LittleEndian, + BigEndian + } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; + switch (machByteOrder) { + case LittleEndian: + { + union { + LittleEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0xFFF00000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + minf = tmpVal.fltVal; + break; + } + + case BigEndian: + { + union { + BigEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0xFFF00000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + minf = tmpVal.fltVal; + break; + } + } + } + + return minf; +} + +/* Function: rtGetMinusInfF ================================================== + * Abstract: + * Initialize rtMinusInfF needed by the generated code. + * Inf is initialized as non-signaling. Assumes IEEE. + */ +real32_T rtGetMinusInfF(void) +{ + IEEESingle minfF; + minfF.wordL.wordLuint = 0xFF800000U; + return minfF.wordL.wordLreal; +} + +/* End of code generation (rtGetInf.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.h b/apps/attitude_estimator_ekf/codegen/rtGetInf.h new file mode 100755 index 000000000..ab2d5a70d --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/rtGetInf.h @@ -0,0 +1,23 @@ +/* + * rtGetInf.h + * + * Code generation for function 'attitudeKalmanfilter' + * + * C source code generated on: Wed Jul 11 08:38:35 2012 + * + */ + +#ifndef __RTGETINF_H__ +#define __RTGETINF_H__ + +#include +#include "rtwtypes.h" +#include "rt_nonfinite.h" + +extern real_T rtGetInf(void); +extern real32_T rtGetInfF(void); +extern real_T rtGetMinusInf(void); +extern real32_T rtGetMinusInfF(void); + +#endif +/* End of code generation (rtGetInf.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c new file mode 100755 index 000000000..17a093461 --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c @@ -0,0 +1,96 @@ +/* + * rtGetNaN.c + * + * Code generation for function 'attitudeKalmanfilter' + * + * C source code generated on: Wed Jul 11 08:38:35 2012 + * + */ + +/* + * Abstract: + * MATLAB for code generation function to initialize non-finite, NaN + */ +#include "rtGetNaN.h" +#define NumBitsPerChar 8U + +/* Function: rtGetNaN ================================================== + * Abstract: + * Initialize rtNaN needed by the generated code. + * NaN is initialized as non-signaling. Assumes IEEE. + */ +real_T rtGetNaN(void) +{ + size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); + real_T nan = 0.0; + if (bitsPerReal == 32U) { + nan = rtGetNaNF(); + } else { + uint16_T one = 1U; + enum { + LittleEndian, + BigEndian + } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; + switch (machByteOrder) { + case LittleEndian: + { + union { + LittleEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0xFFF80000U; + tmpVal.bitVal.words.wordL = 0x00000000U; + nan = tmpVal.fltVal; + break; + } + + case BigEndian: + { + union { + BigEndianIEEEDouble bitVal; + real_T fltVal; + } tmpVal; + + tmpVal.bitVal.words.wordH = 0x7FFFFFFFU; + tmpVal.bitVal.words.wordL = 0xFFFFFFFFU; + nan = tmpVal.fltVal; + break; + } + } + } + + return nan; +} + +/* Function: rtGetNaNF ================================================== + * Abstract: + * Initialize rtNaNF needed by the generated code. + * NaN is initialized as non-signaling. Assumes IEEE. + */ +real32_T rtGetNaNF(void) +{ + IEEESingle nanF = { { 0 } }; + uint16_T one = 1U; + enum { + LittleEndian, + BigEndian + } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; + switch (machByteOrder) { + case LittleEndian: + { + nanF.wordL.wordLuint = 0xFFC00000U; + break; + } + + case BigEndian: + { + nanF.wordL.wordLuint = 0x7FFFFFFFU; + break; + } + } + + return nanF.wordL.wordLreal; +} + +/* End of code generation (rtGetNaN.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h new file mode 100755 index 000000000..2c254bbbf --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h @@ -0,0 +1,21 @@ +/* + * rtGetNaN.h + * + * Code generation for function 'attitudeKalmanfilter' + * + * C source code generated on: Wed Jul 11 08:38:35 2012 + * + */ + +#ifndef __RTGETNAN_H__ +#define __RTGETNAN_H__ + +#include +#include "rtwtypes.h" +#include "rt_nonfinite.h" + +extern real_T rtGetNaN(void); +extern real32_T rtGetNaNF(void); + +#endif +/* End of code generation (rtGetNaN.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c new file mode 100755 index 000000000..005c4f28d --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c @@ -0,0 +1,87 @@ +/* + * rt_nonfinite.c + * + * Code generation for function 'attitudeKalmanfilter' + * + * C source code generated on: Wed Jul 11 08:38:35 2012 + * + */ + +/* + * Abstract: + * MATLAB for code generation function to initialize non-finites, + * (Inf, NaN and -Inf). + */ +#include "rt_nonfinite.h" +#include "rtGetNaN.h" +#include "rtGetInf.h" + +real_T rtInf; +real_T rtMinusInf; +real_T rtNaN; +real32_T rtInfF; +real32_T rtMinusInfF; +real32_T rtNaNF; + +/* Function: rt_InitInfAndNaN ================================================== + * Abstract: + * Initialize the rtInf, rtMinusInf, and rtNaN needed by the + * generated code. NaN is initialized as non-signaling. Assumes IEEE. + */ +void rt_InitInfAndNaN(size_t realSize) +{ + (void) (realSize); + rtNaN = rtGetNaN(); + rtNaNF = rtGetNaNF(); + rtInf = rtGetInf(); + rtInfF = rtGetInfF(); + rtMinusInf = rtGetMinusInf(); + rtMinusInfF = rtGetMinusInfF(); +} + +/* Function: rtIsInf ================================================== + * Abstract: + * Test if value is infinite + */ +boolean_T rtIsInf(real_T value) +{ + return ((value==rtInf || value==rtMinusInf) ? 1U : 0U); +} + +/* Function: rtIsInfF ================================================= + * Abstract: + * Test if single-precision value is infinite + */ +boolean_T rtIsInfF(real32_T value) +{ + return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U); +} + +/* Function: rtIsNaN ================================================== + * Abstract: + * Test if value is not a number + */ +boolean_T rtIsNaN(real_T value) +{ +#if defined(_MSC_VER) && (_MSC_VER <= 1200) + return _isnan(value)? TRUE:FALSE; +#else + return (value!=value)? 1U:0U; +#endif +} + +/* Function: rtIsNaNF ================================================= + * Abstract: + * Test if single-precision value is not a number + */ +boolean_T rtIsNaNF(real32_T value) +{ +#if defined(_MSC_VER) && (_MSC_VER <= 1200) + return _isnan((real_T)value)? true:false; +#else + return (value!=value)? 1U:0U; +#endif +} + + +/* End of code generation (rt_nonfinite.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h new file mode 100755 index 000000000..6481f011f --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h @@ -0,0 +1,53 @@ +/* + * rt_nonfinite.h + * + * Code generation for function 'attitudeKalmanfilter' + * + * C source code generated on: Wed Jul 11 08:38:35 2012 + * + */ + +#ifndef __RT_NONFINITE_H__ +#define __RT_NONFINITE_H__ + +#if defined(_MSC_VER) && (_MSC_VER <= 1200) +#include +#endif +#include +#include "rtwtypes.h" + +extern real_T rtInf; +extern real_T rtMinusInf; +extern real_T rtNaN; +extern real32_T rtInfF; +extern real32_T rtMinusInfF; +extern real32_T rtNaNF; +extern void rt_InitInfAndNaN(size_t realSize); +extern boolean_T rtIsInf(real_T value); +extern boolean_T rtIsInfF(real32_T value); +extern boolean_T rtIsNaN(real_T value); +extern boolean_T rtIsNaNF(real32_T value); + +typedef struct { + struct { + uint32_T wordH; + uint32_T wordL; + } words; +} BigEndianIEEEDouble; + +typedef struct { + struct { + uint32_T wordL; + uint32_T wordH; + } words; +} LittleEndianIEEEDouble; + +typedef struct { + union { + real32_T wordLreal; + uint32_T wordLuint; + } wordL; +} IEEESingle; + +#endif +/* End of code generation (rt_nonfinite.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/rtw_proj.tmw b/apps/attitude_estimator_ekf/codegen/rtw_proj.tmw new file mode 100755 index 000000000..1fb585abd --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/rtw_proj.tmw @@ -0,0 +1 @@ +Code generation project for attitudeKalmanfilter using C:\Program Files\MATLAB\R2011a\toolbox\coder\coder\rtw\c\xrt\xrt_vcx64.tmf. MATLAB root = C:\Program Files\MATLAB\R2011a. diff --git a/apps/attitude_estimator_ekf/codegen/rtwtypes.h b/apps/attitude_estimator_ekf/codegen/rtwtypes.h new file mode 100755 index 000000000..fd629ebcd --- /dev/null +++ b/apps/attitude_estimator_ekf/codegen/rtwtypes.h @@ -0,0 +1,159 @@ +/* + * rtwtypes.h + * + * Code generation for function 'attitudeKalmanfilter' + * + * C source code generated on: Wed Jul 11 08:38:35 2012 + * + */ + +#ifndef __RTWTYPES_H__ +#define __RTWTYPES_H__ +#ifndef TRUE +# define TRUE (1U) +#endif +#ifndef FALSE +# define FALSE (0U) +#endif +#ifndef __TMWTYPES__ +#define __TMWTYPES__ + +#include + +/*=======================================================================* + * Target hardware information + * Device type: Generic->MATLAB Host Computer + * Number of bits: char: 8 short: 16 int: 32 + * long: 32 native word size: 32 + * Byte ordering: LittleEndian + * Signed integer division rounds to: Zero + * Shift right on a signed integer as arithmetic shift: on + *=======================================================================*/ + +/*=======================================================================* + * Fixed width word size data types: * + * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * + * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * + * real32_T, real64_T - 32 and 64 bit floating point numbers * + *=======================================================================*/ + +typedef signed char int8_T; +typedef unsigned char uint8_T; +typedef short int16_T; +typedef unsigned short uint16_T; +typedef int int32_T; +typedef unsigned int uint32_T; +typedef float real32_T; +typedef double real64_T; + +/*===========================================================================* + * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T, * + * ulong_T, char_T and byte_T. * + *===========================================================================*/ + +typedef double real_T; +typedef double time_T; +typedef unsigned char boolean_T; +typedef int int_T; +typedef unsigned int uint_T; +typedef unsigned long ulong_T; +typedef char char_T; +typedef char_T byte_T; + +/*===========================================================================* + * Complex number type definitions * + *===========================================================================*/ +#define CREAL_T + typedef struct { + real32_T re; + real32_T im; + } creal32_T; + + typedef struct { + real64_T re; + real64_T im; + } creal64_T; + + typedef struct { + real_T re; + real_T im; + } creal_T; + + typedef struct { + int8_T re; + int8_T im; + } cint8_T; + + typedef struct { + uint8_T re; + uint8_T im; + } cuint8_T; + + typedef struct { + int16_T re; + int16_T im; + } cint16_T; + + typedef struct { + uint16_T re; + uint16_T im; + } cuint16_T; + + typedef struct { + int32_T re; + int32_T im; + } cint32_T; + + typedef struct { + uint32_T re; + uint32_T im; + } cuint32_T; + + +/*=======================================================================* + * Min and Max: * + * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * + * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * + *=======================================================================*/ + +#define MAX_int8_T ((int8_T)(127)) +#define MIN_int8_T ((int8_T)(-128)) +#define MAX_uint8_T ((uint8_T)(255)) +#define MIN_uint8_T ((uint8_T)(0)) +#define MAX_int16_T ((int16_T)(32767)) +#define MIN_int16_T ((int16_T)(-32768)) +#define MAX_uint16_T ((uint16_T)(65535)) +#define MIN_uint16_T ((uint16_T)(0)) +#define MAX_int32_T ((int32_T)(2147483647)) +#define MIN_int32_T ((int32_T)(-2147483647-1)) +#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) +#define MIN_uint32_T ((uint32_T)(0)) + +/* Logical type definitions */ +#if !defined(__cplusplus) && !defined(__true_false_are_keywords) +# ifndef false +# define false (0U) +# endif +# ifndef true +# define true (1U) +# endif +#endif + +/* + * MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation + * for signed integer values. + */ +#if ((SCHAR_MIN + 1) != -SCHAR_MAX) +#error "This code must be compiled using a 2's complement representation for signed integer values" +#endif + +/* + * Maximum length of a MATLAB identifier (function/variable) + * including the null-termination character. Referenced by + * rt_logging.c and rt_matrx.c. + */ +#define TMW_NAME_LENGTH_MAX 64 + +#endif +#endif +/* End of code generation (rtwtypes.h) */ -- cgit v1.2.3