fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 11:33:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Cut over attitude estimator to new-style config for all boards

Browse Source
This commit is contained in:
Lorenz Meier
2013-04-23 12:36:26 +02:00
parent c52278f679
commit 3ecdca41e5
32 changed files with 26 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.c
Executable
+464
View File
@@ -0,0 +1,464 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Tobias Naegeli <naegelit@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
*
* 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 attitude_estimator_ekf_main.c
*
* Extended Kalman Filter for Attitude Estimation.
*/
#include <nuttx/config.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdbool.h>
#include <poll.h>
#include <fcntl.h>
#include <float.h>
#include <nuttx/sched.h>
#include <sys/prctl.h>
#include <termios.h>
#include <errno.h>
#include <limits.h>
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_hrt.h>
#include <systemlib/systemlib.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include "codegen/attitudeKalmanfilter_initialize.h"
#include "codegen/attitudeKalmanfilter.h"
#include "attitude_estimator_ekf_params.h"
__EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]);
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 */
/**
* Mainloop of attitude_estimator_ekf.
*/
int attitude_estimator_ekf_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: attitude_estimator_ekf {start|stop|status} [-p <additional params>]\n\n");
exit(1);
}
/**
* The attitude_estimator_ekf app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
*/
int attitude_estimator_ekf_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("attitude_estimator_ekf already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
12400,
attitude_estimator_ekf_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tattitude_estimator_ekf app is running\n");
} else {
printf("\tattitude_estimator_ekf app not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
/*
* [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_thread_main(int argc, char *argv[])
{
const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
float dt = 0.005f;
/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */
float x_aposteriori_k[12]; /**< states */
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,
}; /**< init: diagonal matrix with big values */
float x_aposteriori[12];
float P_aposteriori[144];
/* output euler angles */
float euler[3] = {0.0f, 0.0f, 0.0f};
float Rot_matrix[9] = {1.f, 0, 0,
0, 1.f, 0,
0, 0, 1.f
}; /**< init: identity matrix */
// 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;
memset(&raw, 0, sizeof(raw));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_status_s state;
memset(&state, 0, sizeof(state));
uint64_t last_data = 0;
uint64_t last_measurement = 0;
/* subscribe to raw data */
int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
/* rate-limit raw data updates to 200Hz */
orb_set_interval(sub_raw, 4);
/* subscribe to param changes */
int sub_params = orb_subscribe(ORB_ID(parameter_update));
/* subscribe to system state*/
int sub_state = orb_subscribe(ORB_ID(vehicle_status));
/* advertise attitude */
orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
int loopcounter = 0;
int printcounter = 0;
thread_running = true;
/* advertise debug value */
// struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
// orb_advert_t pub_dbg = -1;
float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f};
// XXX write this out to perf regs
/* keep track of sensor updates */
uint32_t sensor_last_count[3] = {0, 0, 0};
uint64_t sensor_last_timestamp[3] = {0, 0, 0};
struct attitude_estimator_ekf_params ekf_params;
struct attitude_estimator_ekf_param_handles ekf_param_handles;
/* initialize parameter handles */
parameters_init(&ekf_param_handles);
uint64_t start_time = hrt_absolute_time();
bool initialized = false;
float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
unsigned offset_count = 0;
/* register the perf counter */
perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf");
/* Main loop*/
while (!thread_should_exit) {
struct pollfd fds[2] = {
{ .fd = sub_raw, .events = POLLIN },
{ .fd = sub_params, .events = POLLIN }
};
int ret = poll(fds, 2, 1000);
if (ret < 0) {
/* XXX this is seriously bad - should be an emergency */
} else if (ret == 0) {
/* check if we're in HIL - not getting sensor data is fine then */
orb_copy(ORB_ID(vehicle_status), sub_state, &state);
if (!state.flag_hil_enabled) {
fprintf(stderr,
"[att ekf] WARNING: Not getting sensors - sensor app running?\n");
}
} else {
/* only update parameters if they changed */
if (fds[1].revents & POLLIN) {
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), sub_params, &update);
/* update parameters */
parameters_update(&ekf_param_handles, &ekf_params);
}
/* only run filter if sensor values changed */
if (fds[0].revents & POLLIN) {
/* get latest measurements */
orb_copy(ORB_ID(sensor_combined), sub_raw, &raw);
if (!initialized) {
gyro_offsets[0] += raw.gyro_rad_s[0];
gyro_offsets[1] += raw.gyro_rad_s[1];
gyro_offsets[2] += raw.gyro_rad_s[2];
offset_count++;
if (hrt_absolute_time() - start_time > 3000000LL) {
initialized = true;
gyro_offsets[0] /= offset_count;
gyro_offsets[1] /= offset_count;
gyro_offsets[2] /= offset_count;
}
} else {
perf_begin(ekf_loop_perf);
/* Calculate data time difference in seconds */
dt = (raw.timestamp - last_measurement) / 1000000.0f;
last_measurement = raw.timestamp;
uint8_t update_vect[3] = {0, 0, 0};
/* Fill in gyro measurements */
if (sensor_last_count[0] != raw.gyro_counter) {
update_vect[0] = 1;
sensor_last_count[0] = raw.gyro_counter;
sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
sensor_last_timestamp[0] = raw.timestamp;
}
z_k[0] = raw.gyro_rad_s[0] - gyro_offsets[0];
z_k[1] = raw.gyro_rad_s[1] - gyro_offsets[1];
z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2];
/* update accelerometer measurements */
if (sensor_last_count[1] != raw.accelerometer_counter) {
update_vect[1] = 1;
sensor_last_count[1] = raw.accelerometer_counter;
sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
sensor_last_timestamp[1] = raw.timestamp;
}
z_k[3] = raw.accelerometer_m_s2[0];
z_k[4] = raw.accelerometer_m_s2[1];
z_k[5] = raw.accelerometer_m_s2[2];
/* update magnetometer measurements */
if (sensor_last_count[2] != raw.magnetometer_counter) {
update_vect[2] = 1;
sensor_last_count[2] = raw.magnetometer_counter;
sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
sensor_last_timestamp[2] = raw.timestamp;
}
z_k[6] = raw.magnetometer_ga[0];
z_k[7] = raw.magnetometer_ga[1];
z_k[8] = raw.magnetometer_ga[2];
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 EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
// overloadcounter = 0;
// }
overloadcounter++;
}
static bool const_initialized = false;
/* initialize with good values once we have a reasonable dt estimate */
if (!const_initialized && dt < 0.05f && dt > 0.005f) {
dt = 0.005f;
parameters_update(&ekf_param_handles, &ekf_params);
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] = 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;
}
/* do not execute the filter if not initialized */
if (!const_initialized) {
continue;
}
uint64_t timing_start = hrt_absolute_time();
attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r,
euler, Rot_matrix, x_aposteriori, P_aposteriori);
/* swap values for next iteration, check for fatal inputs */
if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {
memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k));
memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k));
} else {
/* due to inputs or numerical failure the output is invalid, skip it */
continue;
}
if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data);
last_data = raw.timestamp;
/* send out */
att.timestamp = raw.timestamp;
// XXX Apply the same transformation to the rotation matrix
att.roll = euler[0] - ekf_params.roll_off;
att.pitch = euler[1] - ekf_params.pitch_off;
att.yaw = euler[2] - ekf_params.yaw_off;
att.rollspeed = x_aposteriori[0];
att.pitchspeed = x_aposteriori[1];
att.yawspeed = x_aposteriori[2];
att.rollacc = x_aposteriori[3];
att.pitchacc = x_aposteriori[4];
att.yawacc = x_aposteriori[5];
//att.yawspeed =z_k[2] ;
/* copy offsets */
memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));
/* copy rotation matrix */
memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix));
att.R_valid = true;
if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
// Broadcast
orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
} else {
warnx("NaN in roll/pitch/yaw estimate!");
}
perf_end(ekf_loop_perf);
}
}
}
loopcounter++;
}
thread_running = false;
return 0;
}
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
Executable
+105
View File
@@ -0,0 +1,105 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Tobias Naegeli <naegelit@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
*
* 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 attitude_estimator_ekf_params.c
*
* Parameters for EKF filter
*/
#include "attitude_estimator_ekf_params.h"
/* Extended Kalman Filter covariances */
/* gyro process noise */
PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q0, 1e-4f);
PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q1, 0.08f);
PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q2, 0.009f);
/* gyro offsets process noise */
PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q3, 0.005f);
PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q4, 0.0f);
/* gyro measurement noise */
PARAM_DEFINE_FLOAT(EKF_ATT_V2_R0, 0.0008f);
PARAM_DEFINE_FLOAT(EKF_ATT_V2_R1, 0.8f);
PARAM_DEFINE_FLOAT(EKF_ATT_V2_R2, 1.0f);
/* accelerometer measurement noise */
PARAM_DEFINE_FLOAT(EKF_ATT_V2_R3, 0.0f);
/* offsets in roll, pitch and yaw of sensor plane and body */
PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f);
PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f);
PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f);
int parameters_init(struct attitude_estimator_ekf_param_handles *h)
{
/* PID parameters */
h->q0 = param_find("EKF_ATT_V2_Q0");
h->q1 = param_find("EKF_ATT_V2_Q1");
h->q2 = param_find("EKF_ATT_V2_Q2");
h->q3 = param_find("EKF_ATT_V2_Q3");
h->q4 = param_find("EKF_ATT_V2_Q4");
h->r0 = param_find("EKF_ATT_V2_R0");
h->r1 = param_find("EKF_ATT_V2_R1");
h->r2 = param_find("EKF_ATT_V2_R2");
h->r3 = param_find("EKF_ATT_V2_R3");
h->roll_off = param_find("ATT_ROLL_OFFS");
h->pitch_off = param_find("ATT_PITCH_OFFS");
h->yaw_off = param_find("ATT_YAW_OFFS");
return OK;
}
int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p)
{
param_get(h->q0, &(p->q[0]));
param_get(h->q1, &(p->q[1]));
param_get(h->q2, &(p->q[2]));
param_get(h->q3, &(p->q[3]));
param_get(h->q4, &(p->q[4]));
param_get(h->r0, &(p->r[0]));
param_get(h->r1, &(p->r[1]));
param_get(h->r2, &(p->r[2]));
param_get(h->r3, &(p->r[3]));
param_get(h->roll_off, &(p->roll_off));
param_get(h->pitch_off, &(p->pitch_off));
param_get(h->yaw_off, &(p->yaw_off));
return OK;
}
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
Executable
+68
View File
@@ -0,0 +1,68 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Tobias Naegeli <naegelit@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
*
* 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 attitude_estimator_ekf_params.h
*
* Parameters for EKF filter
*/
#include <systemlib/param/param.h>
struct attitude_estimator_ekf_params {
float r[9];
float q[12];
float roll_off;
float pitch_off;
float yaw_off;
};
struct attitude_estimator_ekf_param_handles {
param_t r0, r1, r2, r3;
param_t q0, q1, q2, q3, q4;
param_t roll_off, pitch_off, yaw_off;
};
/**
* Initialize all parameter handles and values
*
*/
int parameters_init(struct attitude_estimator_ekf_param_handles *h);
/**
* Update all parameters
*
*/
int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p);
src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
Executable
+1148
View File
File diff suppressed because it is too large Load Diff
src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
Executable
+34
View File
@@ -0,0 +1,34 @@
/*
* attitudeKalmanfilter.h
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
#ifndef __ATTITUDEKALMANFILTER_H__
#define __ATTITUDEKALMANFILTER_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
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], 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) */
src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
Executable
+31
View File
@@ -0,0 +1,31 @@
/*
* attitudeKalmanfilter_initialize.c
*
* Code generation for function 'attitudeKalmanfilter_initialize'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
/* 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) */
src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
Executable
+34
View File
@@ -0,0 +1,34 @@
/*
* attitudeKalmanfilter_initialize.h
*
* Code generation for function 'attitudeKalmanfilter_initialize'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
#ifndef __ATTITUDEKALMANFILTER_INITIALIZE_H__
#define __ATTITUDEKALMANFILTER_INITIALIZE_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#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) */
src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
Executable
+31
View File
@@ -0,0 +1,31 @@
/*
* attitudeKalmanfilter_terminate.c
*
* Code generation for function 'attitudeKalmanfilter_terminate'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
/* 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) */
src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
Executable
+34
View File
@@ -0,0 +1,34 @@
/*
* attitudeKalmanfilter_terminate.h
*
* Code generation for function 'attitudeKalmanfilter_terminate'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
#ifndef __ATTITUDEKALMANFILTER_TERMINATE_H__
#define __ATTITUDEKALMANFILTER_TERMINATE_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#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) */
src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
Executable
+16
View File
@@ -0,0 +1,16 @@
/*
* attitudeKalmanfilter_types.h
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
#ifndef __ATTITUDEKALMANFILTER_TYPES_H__
#define __ATTITUDEKALMANFILTER_TYPES_H__
/* Type Definitions */
#endif
/* End of code generation (attitudeKalmanfilter_types.h) */
src/modules/attitude_estimator_ekf/codegen/cross.c
Executable
+37
View File
@@ -0,0 +1,37 @@
/*
* cross.c
*
* Code generation for function 'cross'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "attitudeKalmanfilter.h"
#include "cross.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
/*
*
*/
void cross(const real32_T a[3], const real32_T b[3], real32_T c[3])
{
c[0] = a[1] * b[2] - a[2] * b[1];
c[1] = a[2] * b[0] - a[0] * b[2];
c[2] = a[0] * b[1] - a[1] * b[0];
}
/* End of code generation (cross.c) */
src/modules/attitude_estimator_ekf/codegen/cross.h
Executable
+34
View File
@@ -0,0 +1,34 @@
/*
* cross.h
*
* Code generation for function 'cross'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
#ifndef __CROSS_H__
#define __CROSS_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void cross(const real32_T a[3], const real32_T b[3], real32_T c[3]);
#endif
/* End of code generation (cross.h) */
src/modules/attitude_estimator_ekf/codegen/eye.c
Executable
+51
View File
@@ -0,0 +1,51 @@
/*
* eye.c
*
* Code generation for function 'eye'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
/* 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(&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(&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) */
src/modules/attitude_estimator_ekf/codegen/eye.h
Executable
+35
View File
@@ -0,0 +1,35 @@
/*
* eye.h
*
* Code generation for function 'eye'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
#ifndef __EYE_H__
#define __EYE_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#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) */
src/modules/attitude_estimator_ekf/codegen/mrdivide.c
Executable
+357
View File
@@ -0,0 +1,357 @@
/*
* mrdivide.c
*
* Code generation for function 'mrdivide'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
/* 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 b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36])
{
real32_T b_A[9];
int32_T rtemp;
int32_T k;
real32_T b_B[36];
int32_T r1;
int32_T r2;
int32_T r3;
real32_T maxval;
real32_T a21;
real32_T Y[36];
for (rtemp = 0; rtemp < 3; rtemp++) {
for (k = 0; k < 3; k++) {
b_A[k + 3 * rtemp] = B[rtemp + 3 * k];
}
}
for (rtemp = 0; rtemp < 12; rtemp++) {
for (k = 0; k < 3; k++) {
b_B[k + 3 * rtemp] = A[rtemp + 12 * k];
}
}
r1 = 0;
r2 = 1;
r3 = 2;
maxval = (real32_T)fabs(b_A[0]);
a21 = (real32_T)fabs(b_A[1]);
if (a21 > maxval) {
maxval = a21;
r1 = 1;
r2 = 0;
}
if ((real32_T)fabs(b_A[2]) > maxval) {
r1 = 2;
r2 = 1;
r3 = 0;
}
b_A[r2] /= b_A[r1];
b_A[r3] /= b_A[r1];
b_A[3 + r2] -= b_A[r2] * b_A[3 + r1];
b_A[3 + r3] -= b_A[r3] * b_A[3 + r1];
b_A[6 + r2] -= b_A[r2] * b_A[6 + r1];
b_A[6 + r3] -= b_A[r3] * b_A[6 + r1];
if ((real32_T)fabs(b_A[3 + r3]) > (real32_T)fabs(b_A[3 + r2])) {
rtemp = r2;
r2 = r3;
r3 = rtemp;
}
b_A[3 + r3] /= b_A[3 + r2];
b_A[6 + r3] -= b_A[3 + r3] * b_A[6 + r2];
for (k = 0; k < 12; k++) {
Y[3 * k] = b_B[r1 + 3 * k];
Y[1 + 3 * k] = b_B[r2 + 3 * k] - Y[3 * k] * b_A[r2];
Y[2 + 3 * k] = (b_B[r3 + 3 * k] - Y[3 * k] * b_A[r3]) - Y[1 + 3 * k] * b_A[3
+ r3];
Y[2 + 3 * k] /= b_A[6 + r3];
Y[3 * k] -= Y[2 + 3 * k] * b_A[6 + r1];
Y[1 + 3 * k] -= Y[2 + 3 * k] * b_A[6 + r2];
Y[1 + 3 * k] /= b_A[3 + r2];
Y[3 * k] -= Y[1 + 3 * k] * b_A[3 + r1];
Y[3 * k] /= b_A[r1];
}
for (rtemp = 0; rtemp < 3; rtemp++) {
for (k = 0; k < 12; k++) {
y[k + 12 * rtemp] = Y[rtemp + 3 * k];
}
}
}
/*
*
*/
void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72])
{
real32_T b_A[36];
int8_T ipiv[6];
int32_T i3;
int32_T iy;
int32_T j;
int32_T c;
int32_T ix;
real32_T temp;
int32_T k;
real32_T s;
int32_T jy;
int32_T ijA;
real32_T Y[72];
for (i3 = 0; i3 < 6; i3++) {
for (iy = 0; iy < 6; iy++) {
b_A[iy + 6 * i3] = B[i3 + 6 * iy];
}
ipiv[i3] = (int8_T)(1 + i3);
}
for (j = 0; j < 5; j++) {
c = j * 7;
iy = 0;
ix = c;
temp = (real32_T)fabs(b_A[c]);
for (k = 2; k <= 6 - j; k++) {
ix++;
s = (real32_T)fabs(b_A[ix]);
if (s > temp) {
iy = k - 1;
temp = s;
}
}
if (b_A[c + iy] != 0.0F) {
if (iy != 0) {
ipiv[j] = (int8_T)((j + iy) + 1);
ix = j;
iy += j;
for (k = 0; k < 6; k++) {
temp = b_A[ix];
b_A[ix] = b_A[iy];
b_A[iy] = temp;
ix += 6;
iy += 6;
}
}
i3 = (c - j) + 6;
for (jy = c + 1; jy + 1 <= i3; jy++) {
b_A[jy] /= b_A[c];
}
}
iy = c;
jy = c + 6;
for (k = 1; k <= 5 - j; k++) {
temp = b_A[jy];
if (b_A[jy] != 0.0F) {
ix = c + 1;
i3 = (iy - j) + 12;
for (ijA = 7 + iy; ijA + 1 <= i3; ijA++) {
b_A[ijA] += b_A[ix] * -temp;
ix++;
}
}
jy += 6;
iy += 6;
}
}
for (i3 = 0; i3 < 12; i3++) {
for (iy = 0; iy < 6; iy++) {
Y[iy + 6 * i3] = A[i3 + 12 * iy];
}
}
for (jy = 0; jy < 6; jy++) {
if (ipiv[jy] != jy + 1) {
for (j = 0; j < 12; j++) {
temp = Y[jy + 6 * j];
Y[jy + 6 * j] = Y[(ipiv[jy] + 6 * j) - 1];
Y[(ipiv[jy] + 6 * j) - 1] = temp;
}
}
}
for (j = 0; j < 12; j++) {
c = 6 * j;
for (k = 0; k < 6; k++) {
iy = 6 * k;
if (Y[k + c] != 0.0F) {
for (jy = k + 2; jy < 7; jy++) {
Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1];
}
}
}
}
for (j = 0; j < 12; j++) {
c = 6 * j;
for (k = 5; k > -1; k += -1) {
iy = 6 * k;
if (Y[k + c] != 0.0F) {
Y[k + c] /= b_A[k + iy];
for (jy = 0; jy + 1 <= k; jy++) {
Y[jy + c] -= Y[k + c] * b_A[jy + iy];
}
}
}
}
for (i3 = 0; i3 < 6; i3++) {
for (iy = 0; iy < 12; iy++) {
y[iy + 12 * i3] = Y[i3 + 6 * iy];
}
}
}
/*
*
*/
void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])
{
real32_T b_A[81];
int8_T ipiv[9];
int32_T i2;
int32_T iy;
int32_T j;
int32_T c;
int32_T ix;
real32_T temp;
int32_T k;
real32_T s;
int32_T jy;
int32_T ijA;
real32_T Y[108];
for (i2 = 0; i2 < 9; i2++) {
for (iy = 0; iy < 9; iy++) {
b_A[iy + 9 * i2] = B[i2 + 9 * iy];
}
ipiv[i2] = (int8_T)(1 + i2);
}
for (j = 0; j < 8; j++) {
c = j * 10;
iy = 0;
ix = c;
temp = (real32_T)fabs(b_A[c]);
for (k = 2; k <= 9 - j; k++) {
ix++;
s = (real32_T)fabs(b_A[ix]);
if (s > temp) {
iy = k - 1;
temp = s;
}
}
if (b_A[c + iy] != 0.0F) {
if (iy != 0) {
ipiv[j] = (int8_T)((j + iy) + 1);
ix = j;
iy += j;
for (k = 0; k < 9; k++) {
temp = b_A[ix];
b_A[ix] = b_A[iy];
b_A[iy] = temp;
ix += 9;
iy += 9;
}
}
i2 = (c - j) + 9;
for (jy = c + 1; jy + 1 <= i2; jy++) {
b_A[jy] /= b_A[c];
}
}
iy = c;
jy = c + 9;
for (k = 1; k <= 8 - j; k++) {
temp = b_A[jy];
if (b_A[jy] != 0.0F) {
ix = c + 1;
i2 = (iy - j) + 18;
for (ijA = 10 + iy; ijA + 1 <= i2; ijA++) {
b_A[ijA] += b_A[ix] * -temp;
ix++;
}
}
jy += 9;
iy += 9;
}
}
for (i2 = 0; i2 < 12; i2++) {
for (iy = 0; iy < 9; iy++) {
Y[iy + 9 * i2] = A[i2 + 12 * iy];
}
}
for (jy = 0; jy < 9; jy++) {
if (ipiv[jy] != jy + 1) {
for (j = 0; j < 12; j++) {
temp = Y[jy + 9 * j];
Y[jy + 9 * j] = Y[(ipiv[jy] + 9 * j) - 1];
Y[(ipiv[jy] + 9 * j) - 1] = temp;
}
}
}
for (j = 0; j < 12; j++) {
c = 9 * j;
for (k = 0; k < 9; k++) {
iy = 9 * k;
if (Y[k + c] != 0.0F) {
for (jy = k + 2; jy < 10; jy++) {
Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1];
}
}
}
}
for (j = 0; j < 12; j++) {
c = 9 * j;
for (k = 8; k > -1; k += -1) {
iy = 9 * k;
if (Y[k + c] != 0.0F) {
Y[k + c] /= b_A[k + iy];
for (jy = 0; jy + 1 <= k; jy++) {
Y[jy + c] -= Y[k + c] * b_A[jy + iy];
}
}
}
}
for (i2 = 0; i2 < 9; i2++) {
for (iy = 0; iy < 12; iy++) {
y[iy + 12 * i2] = Y[i2 + 9 * iy];
}
}
}
/* End of code generation (mrdivide.c) */
src/modules/attitude_estimator_ekf/codegen/mrdivide.h
Executable
+36
View File
@@ -0,0 +1,36 @@
/*
* mrdivide.h
*
* Code generation for function 'mrdivide'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
#ifndef __MRDIVIDE_H__
#define __MRDIVIDE_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
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) */
src/modules/attitude_estimator_ekf/codegen/norm.c
Executable
+54
View File
@@ -0,0 +1,54 @@
/*
* norm.c
*
* Code generation for function 'norm'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
/* 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;
int32_T k;
real32_T absxk;
real32_T t;
y = 0.0F;
scale = 1.17549435E-38F;
for (k = 0; k < 3; k++) {
absxk = (real32_T)fabs(x[k]);
if (absxk > scale) {
t = scale / absxk;
y = 1.0F + y * t * t;
scale = absxk;
} else {
t = absxk / scale;
y += t * t;
}
}
return scale * (real32_T)sqrt(y);
}
/* End of code generation (norm.c) */
src/modules/attitude_estimator_ekf/codegen/norm.h
Executable
+34
View File
@@ -0,0 +1,34 @@
/*
* norm.h
*
* Code generation for function 'norm'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
#ifndef __NORM_H__
#define __NORM_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#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) */
src/modules/attitude_estimator_ekf/codegen/rdivide.c
Executable
+38
View File
@@ -0,0 +1,38 @@
/*
* rdivide.c
*
* Code generation for function 'rdivide'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "attitudeKalmanfilter.h"
#include "rdivide.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
/*
*
*/
void rdivide(const real32_T x[3], real32_T y, real32_T z[3])
{
int32_T i;
for (i = 0; i < 3; i++) {
z[i] = x[i] / y;
}
}
/* End of code generation (rdivide.c) */
src/modules/attitude_estimator_ekf/codegen/rdivide.h
Executable
+34
View File
@@ -0,0 +1,34 @@
/*
* rdivide.h
*
* Code generation for function 'rdivide'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
#ifndef __RDIVIDE_H__
#define __RDIVIDE_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void rdivide(const real32_T x[3], real32_T y, real32_T z[3]);
#endif
/* End of code generation (rdivide.h) */
src/modules/attitude_estimator_ekf/codegen/rtGetInf.c
Executable
+139
View File
@@ -0,0 +1,139 @@
/*
* rtGetInf.c
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
/*
* 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) */
src/modules/attitude_estimator_ekf/codegen/rtGetInf.h
Executable
+23
View File
@@ -0,0 +1,23 @@
/*
* rtGetInf.h
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
#ifndef __RTGETINF_H__
#define __RTGETINF_H__
#include <stddef.h>
#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) */
src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c
Executable
+96
View File
@@ -0,0 +1,96 @@
/*
* rtGetNaN.c
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
/*
* 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) */
src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h
Executable
+21
View File
@@ -0,0 +1,21 @@
/*
* rtGetNaN.h
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
#ifndef __RTGETNAN_H__
#define __RTGETNAN_H__
#include <stddef.h>
#include "rtwtypes.h"
#include "rt_nonfinite.h"
extern real_T rtGetNaN(void);
extern real32_T rtGetNaNF(void);
#endif
/* End of code generation (rtGetNaN.h) */
src/modules/attitude_estimator_ekf/codegen/rt_defines.h
Executable
+24
View File
@@ -0,0 +1,24 @@
/*
* rt_defines.h
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
#ifndef __RT_DEFINES_H__
#define __RT_DEFINES_H__
#include <stdlib.h>
#define RT_PI 3.14159265358979323846
#define RT_PIF 3.1415927F
#define RT_LN_10 2.30258509299404568402
#define RT_LN_10F 2.3025851F
#define RT_LOG10E 0.43429448190325182765
#define RT_LOG10EF 0.43429449F
#define RT_E 2.7182818284590452354
#define RT_EF 2.7182817F
#endif
/* End of code generation (rt_defines.h) */
src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c
Executable
+87
View File
@@ -0,0 +1,87 @@
/*
* rt_nonfinite.c
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
/*
* 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) */
src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h
Executable
+53
View File
@@ -0,0 +1,53 @@
/*
* rt_nonfinite.h
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
#ifndef __RT_NONFINITE_H__
#define __RT_NONFINITE_H__
#if defined(_MSC_VER) && (_MSC_VER <= 1200)
#include <float.h>
#endif
#include <stddef.h>
#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) */
src/modules/attitude_estimator_ekf/codegen/rtwtypes.h
Executable
+159
View File
@@ -0,0 +1,159 @@
/*
* rtwtypes.h
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
#ifndef __RTWTYPES_H__
#define __RTWTYPES_H__
#ifndef TRUE
# define TRUE (1U)
#endif
#ifndef FALSE
# define FALSE (0U)
#endif
#ifndef __TMWTYPES__
#define __TMWTYPES__
#include <limits.h>
/*=======================================================================*
* 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) */
src/modules/attitude_estimator_ekf/module.mk
+16
View File
@@ -0,0 +1,16 @@
MODULE_NAME = attitude_estimator_ekf
SRCS = attitude_estimator_ekf_main.c \
attitude_estimator_ekf_params.c \
codegen/attitudeKalmanfilter_initialize.c \
codegen/attitudeKalmanfilter_terminate.c \
codegen/attitudeKalmanfilter.c \
codegen/cross.c \
codegen/eye.c \
codegen/mrdivide.c \
codegen/norm.c \
codegen/rdivide.c \
codegen/rt_nonfinite.c \
codegen/rtGetInf.c \
codegen/rtGetNaN.c