diff options
Diffstat (limited to 'apps/px4')
-rw-r--r-- | apps/px4/attitude_estimator_bm/.context | 0 | ||||
-rw-r--r-- | apps/px4/attitude_estimator_bm/Makefile | 42 | ||||
-rw-r--r-- | apps/px4/attitude_estimator_bm/attitude_bm.c | 322 | ||||
-rw-r--r-- | apps/px4/attitude_estimator_bm/attitude_bm.h | 24 | ||||
-rw-r--r-- | apps/px4/attitude_estimator_bm/attitude_estimator_bm.c | 283 | ||||
-rw-r--r-- | apps/px4/attitude_estimator_bm/kalman.c | 115 | ||||
-rw-r--r-- | apps/px4/attitude_estimator_bm/kalman.h | 35 | ||||
-rw-r--r-- | apps/px4/attitude_estimator_bm/matrix.h | 156 | ||||
-rw-r--r-- | apps/px4/ground_estimator/Makefile | 42 | ||||
-rw-r--r-- | apps/px4/ground_estimator/ground_estimator.c | 180 |
10 files changed, 0 insertions, 1199 deletions
diff --git a/apps/px4/attitude_estimator_bm/.context b/apps/px4/attitude_estimator_bm/.context deleted file mode 100644 index e69de29bb..000000000 --- a/apps/px4/attitude_estimator_bm/.context +++ /dev/null diff --git a/apps/px4/attitude_estimator_bm/Makefile b/apps/px4/attitude_estimator_bm/Makefile deleted file mode 100644 index 2c1cfc510..000000000 --- a/apps/px4/attitude_estimator_bm/Makefile +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# 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. -# -############################################################################ - -# -# Makefile to build the black magic attitude estimator -# - -APPNAME = attitude_estimator_bm -PRIORITY = SCHED_PRIORITY_MAX - 10 -STACKSIZE = 12000 - -include $(APPDIR)/mk/app.mk diff --git a/apps/px4/attitude_estimator_bm/attitude_bm.c b/apps/px4/attitude_estimator_bm/attitude_bm.c deleted file mode 100644 index 6921db375..000000000 --- a/apps/px4/attitude_estimator_bm/attitude_bm.c +++ /dev/null @@ -1,322 +0,0 @@ - -/* - * attitude_bm.c - * - * Created on: 21.12.2010 - * Author: Laurens Mackay, Tobias Naegeli - */ - -#include <math.h> -#include "attitude_bm.h" -#include "kalman.h" - - -#define TIME_STEP (1.0f / 500.0f) - -static kalman_t attitude_blackmagic_kal; - -void vect_norm(float_vect3 *vect) -{ - float length = sqrtf( - vect->x * vect->x + vect->y * vect->y + vect->z * vect->z); - - if (length != 0) { - vect->x /= length; - vect->y /= length; - vect->z /= length; - } -} - - -void vect_cross_product(const float_vect3 *a, const float_vect3 *b, - float_vect3 *c) -{ - c->x = a->y * b->z - a->z * b->y; - c->y = a->z * b->x - a->x * b->z; - c->z = a->x * b->y - a->y * b->x; -} - -void attitude_blackmagic_update_a(void) -{ - // for acc - // Idendity matrix already in A. - M(attitude_blackmagic_kal.a, 0, 1) = TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 11); - M(attitude_blackmagic_kal.a, 0, 2) = -TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 10); - - M(attitude_blackmagic_kal.a, 1, 0) = -TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 11); - M(attitude_blackmagic_kal.a, 1, 2) = TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 9); - - M(attitude_blackmagic_kal.a, 2, 0) = TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 10); - M(attitude_blackmagic_kal.a, 2, 1) = -TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 9); - - // for mag - // Idendity matrix already in A. - M(attitude_blackmagic_kal.a, 3, 4) = TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 11); - M(attitude_blackmagic_kal.a, 3, 5) = -TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 10); - - M(attitude_blackmagic_kal.a, 4, 3) = -TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 11); - M(attitude_blackmagic_kal.a, 4, 5) = TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 9); - - M(attitude_blackmagic_kal.a, 5, 3) = TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 10); - M(attitude_blackmagic_kal.a, 5, 4) = -TIME_STEP * kalman_get_state( - &attitude_blackmagic_kal, 9); - -} - -void attitude_blackmagic_init(void) -{ - //X Kalmanfilter - //initalize matrices - - static m_elem kal_a[12 * 12] = { - 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - - 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - - 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, - - 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, - - 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, - - 0, 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, - - 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, - - 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, - - 0, 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 0, - - 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, - - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, - - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0f - }; - - static m_elem kal_c[9 * 12] = { - 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - - 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - - 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, - - 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, - - 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, - - 0, 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, - - 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 1.0f, 0, 0, - - 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 1.0f, 0, - - 0, 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 1.0f - }; - - - -#define FACTOR 0.5 -#define FACTORstart 1 - - -// static m_elem kal_gain[12 * 9] = -// { 0.004 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, -// 0 , 0.004 , 0 , 0 , 0 , 0 , 0 , 0 , 0, -// 0 , 0 , 0.004 , 0 , 0 , 0 , 0 , 0 , 0, -// 0 , 0 , 0 , 0.015, 0 , 0 , 0 , 0 , 0, -// 0 , 0 , 0 , 0 , 0.015, 0 , 0 , 0 , 0, -// 0 , 0 , 0 , 0 , 0 , 0.015, 0 , 0 , 0, -// 0.0000 , +0.00002,0 , 0 , 0, 0, 0, 0 , 0, -// -0.00002,0 , 0 , 0 , 0, 0, 0, 0, 0, -// 0, 0 , 0 , 0, 0, 0, 0, 0, 0, -// 0 , 0 , 0 , 0 , 0 , 0 , 0.4 , 0 , 0, -// 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.4 , 0, -// 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.4 -// }; - - static m_elem kal_gain[12 * 9] = { - 0.0007f , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, - 0 , 0.0007f , 0 , 0 , 0 , 0 , 0 , 0 , 0, - 0 , 0 , 0.0007f , 0 , 0 , 0 , 0 , 0 , 0, - 0 , 0 , 0 , 0.015f, 0 , 0 , 0 , 0 , 0, - 0 , 0 , 0 , 0 , 0.015f, 0 , 0 , 0 , 0, - 0 , 0 , 0 , 0 , 0 , 0.015f, 0 , 0 , 0, - 0.0000f , +0.00002f, 0 , 0 , 0, 0, 0, 0 , 0, - -0.00002f, 0 , 0 , 0 , 0, 0, 0, 0, 0, - 0, 0 , 0 , 0, 0, 0, 0, 0, 0, - 0 , 0 , 0 , 0 , 0 , 0 , 0.7f , 0 , 0, - 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.7f , 0, - 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.7f - }; - //offset update only correct if not upside down. - -#define K (10.0f*TIME_STEP) - - static m_elem kal_gain_start[12 * 9] = { - K, 0, 0, 0, 0, 0, 0, 0, 0, - - 0, K, 0, 0, 0, 0, 0, 0, 0, - - 0, 0, K, 0, 0, 0, 0, 0, 0, - - 0, 0, 0, K, 0, 0, 0, 0, 0, - - 0, 0, 0, 0, K, 0, 0, 0, 0, - - 0, 0, 0, 0, 0, K, 0, 0, 0, - - 0, 0, 0, 0, 0, 0, K, 0, 0, - - 0, 0, 0, 0, 0, 0, 0, K, 0, - - 0, 0, 0, 0, 0, 0, 0, 0, K, - - 0, 0, 0, 0, 0, 0, 0, 0, 0, - - 0, 0, 0, 0, 0, 0, 0, 0, 0, - - 0, 0, 0, 0, 0, 0, 0, 0, 0 - }; - - - - static m_elem kal_x_apriori[12 * 1] = - { }; - - - //---> initial states sind aposteriori!? ---> fehler - static m_elem kal_x_aposteriori[12 * 1] = - { 0.0f, 0.0f, -1.0f, 0.6f, 0.0f, 0.8f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f }; - - kalman_init(&attitude_blackmagic_kal, 12, 9, kal_a, kal_c, - kal_gain_start, kal_gain, kal_x_apriori, kal_x_aposteriori, 1000); - -} - -void attitude_blackmagic(const float_vect3 *accel, const float_vect3 *mag, const float_vect3 *gyro) -{ - // Kalman Filter - - //Calculate new linearized A matrix - attitude_blackmagic_update_a(); - - kalman_predict(&attitude_blackmagic_kal); - - //correction update - - m_elem measurement[9] = - { }; - m_elem mask[9] = - { 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f }; - - // XXX Hack - stop updating accel if upside down - - if (accel->z > 0) { - mask[0] = 0.0f; - mask[1] = 0.0f; - mask[2] = 0.0f; - } else { - mask[0] = 1.0f; - mask[1] = 1.0f; - mask[2] = 1.0f; - } - - measurement[0] = accel->x; - measurement[1] = accel->y; - measurement[2] = accel->z; - - measurement[3] = mag->x; - measurement[4] = mag->y; - measurement[5] = mag->z; - - measurement[6] = gyro->x; - measurement[7] = gyro->y; - measurement[8] = gyro->z; - - //Put measurements into filter - - -// static int j = 0; -// if (j >= 3) -// { -// j = 0; -// -// mask[3]=1; -// mask[4]=1; -// mask[5]=1; -// j=0; -// -// }else{ -// j++;} - - kalman_correct(&attitude_blackmagic_kal, measurement, mask); - -} -void attitude_blackmagic_get_all(float_vect3 *euler, float_vect3 *rates, float_vect3 *x_n_b, float_vect3 *y_n_b, float_vect3 *z_n_b) -{ - //debug - - // save outputs - float_vect3 kal_acc; - float_vect3 kal_mag; -// float_vect3 kal_w0, kal_w; - - kal_acc.x = kalman_get_state(&attitude_blackmagic_kal, 0); - kal_acc.y = kalman_get_state(&attitude_blackmagic_kal, 1); - kal_acc.z = kalman_get_state(&attitude_blackmagic_kal, 2); - - kal_mag.x = kalman_get_state(&attitude_blackmagic_kal, 3); - kal_mag.y = kalman_get_state(&attitude_blackmagic_kal, 4); - kal_mag.z = kalman_get_state(&attitude_blackmagic_kal, 5); - -// kal_w0.x = kalman_get_state(&attitude_blackmagic_kal, 6); -// kal_w0.y = kalman_get_state(&attitude_blackmagic_kal, 7); -// kal_w0.z = kalman_get_state(&attitude_blackmagic_kal, 8); -// -// kal_w.x = kalman_get_state(&attitude_blackmagic_kal, 9); -// kal_w.y = kalman_get_state(&attitude_blackmagic_kal, 10); -// kal_w.z = kalman_get_state(&attitude_blackmagic_kal, 11); - - rates->x = kalman_get_state(&attitude_blackmagic_kal, 9); - rates->y = kalman_get_state(&attitude_blackmagic_kal, 10); - rates->z = kalman_get_state(&attitude_blackmagic_kal, 11); - - - -// kal_w = kal_w; // XXX hack to silence compiler warning -// kal_w0 = kal_w0; // XXX hack to silence compiler warning - - - - //debug_vect("magn", mag); - - //float_vect3 x_n_b, y_n_b, z_n_b; - z_n_b->x = -kal_acc.x; - z_n_b->y = -kal_acc.y; - z_n_b->z = -kal_acc.z; - vect_norm(z_n_b); - vect_cross_product(z_n_b, &kal_mag, y_n_b); - vect_norm(y_n_b); - - vect_cross_product(y_n_b, z_n_b, x_n_b); - - - - //save euler angles - euler->x = atan2f(z_n_b->y, z_n_b->z); - euler->y = -asinf(z_n_b->x); - euler->z = atan2f(y_n_b->x, x_n_b->x); - -} diff --git a/apps/px4/attitude_estimator_bm/attitude_bm.h b/apps/px4/attitude_estimator_bm/attitude_bm.h deleted file mode 100644 index c21b3d6f1..000000000 --- a/apps/px4/attitude_estimator_bm/attitude_bm.h +++ /dev/null @@ -1,24 +0,0 @@ -/* - * attitude_blackmagic.h - * - * Created on: May 31, 2011 - * Author: pixhawk - */ - -#ifndef attitude_blackmagic_H_ -#define attitude_blackmagic_H_ - -#include "matrix.h" - -void vect_norm(float_vect3 *vect); - -void vect_cross_product(const float_vect3 *a, const float_vect3 *b, float_vect3 *c); - -void attitude_blackmagic_update_a(void); - -void attitude_blackmagic_init(void); - -void attitude_blackmagic(const float_vect3 *accel, const float_vect3 *mag, const float_vect3 *gyro); - -void attitude_blackmagic_get_all(float_vect3 *euler, float_vect3 *rates, float_vect3 *x_n_b, float_vect3 *y_n_b, float_vect3 *z_n_b); -#endif /* attitude_blackmagic_H_ */ diff --git a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c deleted file mode 100644 index 60737a654..000000000 --- a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c +++ /dev/null @@ -1,283 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli <naegelit@student.ethz.ch> - * Laurens Mackay <mackayl@student.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_bm.c - * Black Magic Attitude Estimator - */ - - - -#include <nuttx/config.h> -#include <stdio.h> -#include <unistd.h> -#include <sys/time.h> -#include <stdbool.h> -#include <fcntl.h> -#include <drivers/drv_hrt.h> -#include <string.h> -#include <poll.h> -#include <uORB/uORB.h> -#include <uORB/topics/sensor_combined.h> -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_status.h> -#include <math.h> -#include <errno.h> - -#include "attitude_bm.h" - -static unsigned int loop_interval_alarm = 4500; // loop interval in microseconds - -__EXPORT int attitude_estimator_bm_main(int argc, char *argv[]); - -/**************************************************************************** - * Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Data - ****************************************************************************/ - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * user_start - ****************************************************************************/ -int attitude_estimator_bm_update(struct sensor_combined_s *raw, float_vect3 *euler, float_vect3 *rates, float_vect3 *x_n_b, float_vect3 *y_n_b, float_vect3 *z_n_b); - -int attitude_estimator_bm_update(struct sensor_combined_s *raw, float_vect3 *euler, float_vect3 *rates, float_vect3 *x_n_b, float_vect3 *y_n_b, float_vect3 *z_n_b) -{ - float_vect3 gyro_values; - gyro_values.x = raw->gyro_rad_s[0]; - gyro_values.y = raw->gyro_rad_s[1]; - gyro_values.z = raw->gyro_rad_s[2]; - - float_vect3 accel_values; - accel_values.x = (raw->accelerometer_m_s2[0] / 9.81f) * 100; - accel_values.y = (raw->accelerometer_m_s2[1] / 9.81f) * 100; - accel_values.z = (raw->accelerometer_m_s2[2] / 9.81f) * 100; - - float_vect3 mag_values; - mag_values.x = raw->magnetometer_ga[0]*1500.0f; - mag_values.y = raw->magnetometer_ga[1]*1500.0f; - mag_values.z = raw->magnetometer_ga[2]*1500.0f; - - // static int i = 0; - - // if (i == 500) { - // printf("[att estim bm] gyro: %8.4f\t%8.4f\t%8.4f\t accel: %8.4f\t%8.4f\t%8.4f\t mag: %8.4f\t%8.4f\t%8.4f\t\n", - // gyro_values.x, gyro_values.y, gyro_values.z, - // accel_values.x, accel_values.y, accel_values.z, - // mag_values.x, mag_values.y, mag_values.z); - // i = 0; - // } - // i++; - - attitude_blackmagic(&accel_values, &mag_values, &gyro_values); - - /* read out values */ - attitude_blackmagic_get_all(euler, rates, x_n_b, y_n_b, z_n_b); - - return OK; -} - - -int attitude_estimator_bm_main(int argc, char *argv[]) -{ - // print text - printf("Black Magic Attitude Estimator initialized..\n\n"); - fflush(stdout); - - /* data structures to read euler angles and rotation matrix back */ - float_vect3 euler = {.x = 0, .y = 0, .z = 0}; - float_vect3 rates; - float_vect3 x_n_b; - float_vect3 y_n_b; - float_vect3 z_n_b; - - int overloadcounter = 19; - - /* initialize */ - attitude_blackmagic_init(); - - /* store start time to guard against too slow update rates */ - uint64_t last_run = hrt_absolute_time(); - - struct sensor_combined_s sensor_combined_s_local = { .gyro_raw = {0}}; - struct vehicle_attitude_s att = {.roll = 0.0f, .pitch = 0.0f, .yaw = 0.0f, - .rollspeed = 0.0f, .pitchspeed = 0.0f, .yawspeed = 0.0f, - .R = {0}, .timestamp = 0}; - - uint64_t last_data = 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, 5); - - bool hil_enabled = false; - bool publishing = false; - - /* advertise attitude */ - orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); - publishing = true; - - struct pollfd fds[] = { - { .fd = sub_raw, .events = POLLIN }, - }; - - /* subscribe to system status */ - struct vehicle_status_s vstatus = {0}; - int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); - - unsigned int loopcounter = 0; - - uint64_t last_checkstate_stamp = 0; - - /* Main loop*/ - while (true) { - - - /* wait for sensor update */ - int ret = poll(fds, 1, 1000); - - if (ret < 0) { - /* XXX this is seriously bad - should be an emergency */ - } else if (ret == 0) { - /* XXX this means no sensor data - should be critical or emergency */ - printf("[attitude estimator bm] WARNING: Not getting sensor data - sensor app running?\n"); - } else { - orb_copy(ORB_ID(sensor_combined), sub_raw, &sensor_combined_s_local); - - uint64_t now = hrt_absolute_time(); - unsigned int time_elapsed = now - last_run; - last_run = now; - - //#if 0 - - 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++; - } - - //#endif - // now = hrt_absolute_time(); - /* filter values */ - attitude_estimator_bm_update(&sensor_combined_s_local, &euler, &rates, &x_n_b, &y_n_b, &z_n_b); - - // time_elapsed = hrt_absolute_time() - now; - // if (blubb == 20) - // { - // printf("Estimator: %lu\n", time_elapsed); - // blubb = 0; - // } - // blubb++; - - // if (last_data > 0 && sensor_combined_s_local.timestamp - last_data > 8000) printf("sensor data missed! (%llu)\n", sensor_combined_s_local.timestamp - last_data); - - // printf("%llu -> %llu = %llu\n", last_data, sensor_combined_s_local.timestamp, sensor_combined_s_local.timestamp - last_data); - // last_data = sensor_combined_s_local.timestamp; - - /*correct yaw */ - // euler.z = euler.z + M_PI; - - /* send out */ - - att.timestamp = sensor_combined_s_local.timestamp; - att.roll = euler.x; - att.pitch = euler.y; - att.yaw = euler.z; - - if (att.yaw > M_PI_F) { - att.yaw -= 2.0f * M_PI_F; - } - - if (att.yaw < -M_PI_F) { - att.yaw += 2.0f * M_PI_F; - } - - att.rollspeed = rates.x; - att.pitchspeed = rates.y; - att.yawspeed = rates.z; - - att.R[0][0] = x_n_b.x; - att.R[0][1] = x_n_b.y; - att.R[0][2] = x_n_b.z; - - // Broadcast - if (publishing) orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); - } - - - // XXX add remaining entries - - if (hrt_absolute_time() - last_checkstate_stamp > 500000) { - /* Check HIL state */ - orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus); - /* switching from non-HIL to HIL mode */ - //printf("[attitude_estimator_bm] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled); - if (vstatus.flag_hil_enabled && !hil_enabled) { - hil_enabled = true; - publishing = false; - int ret = close(pub_att); - printf("Closing attitude: %i \n", ret); - - /* switching from HIL to non-HIL mode */ - - } else if (!publishing && !hil_enabled) { - /* advertise the topic and make the initial publication */ - pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); - hil_enabled = false; - publishing = true; - } - last_checkstate_stamp = hrt_absolute_time(); - } - - loopcounter++; - } - - /* Should never reach here */ - return 0; -} - - diff --git a/apps/px4/attitude_estimator_bm/kalman.c b/apps/px4/attitude_estimator_bm/kalman.c deleted file mode 100644 index e4ea7a417..000000000 --- a/apps/px4/attitude_estimator_bm/kalman.c +++ /dev/null @@ -1,115 +0,0 @@ -/* - * kalman.c - * - * Created on: 01.12.2010 - * Author: Laurens Mackay - */ -#include "kalman.h" -//#include "mavlink_debug.h" - -void kalman_init(kalman_t *kalman, int states, int measurements, m_elem a[], - m_elem c[], m_elem gain_start[], m_elem gain[], m_elem x_apriori[], - m_elem x_aposteriori[], int gainfactorsteps) -{ - kalman->states = states; - kalman->measurements = measurements; - kalman->gainfactorsteps = gainfactorsteps; - kalman->gainfactor = 0; - - //Create all matrices that are persistent - kalman->a = matrix_create(states, states, a); - kalman->c = matrix_create(measurements, states, c); - kalman->gain_start = matrix_create(states, measurements, gain_start); - kalman->gain = matrix_create(states, measurements, gain); - kalman->x_apriori = matrix_create(states, 1, x_apriori); - kalman->x_aposteriori = matrix_create(states, 1, x_aposteriori); -} - -void kalman_predict(kalman_t *kalman) -{ - matrix_mult(kalman->a, kalman->x_aposteriori, kalman->x_apriori); -} - -void kalman_correct(kalman_t *kalman, m_elem measurement_a[], m_elem mask_a[]) -{ - //create matrices from inputs - matrix_t measurement = - matrix_create(kalman->measurements, 1, measurement_a); - matrix_t mask = matrix_create(kalman->measurements, 1, mask_a); - - //create temporary matrices - m_elem gain_start_part_a[KALMAN_MAX_STATES * KALMAN_MAX_MEASUREMENTS] = - { }; - matrix_t gain_start_part = matrix_create(kalman->states, - kalman->measurements, gain_start_part_a); - - m_elem gain_part_a[KALMAN_MAX_STATES * KALMAN_MAX_MEASUREMENTS] = - { }; - matrix_t gain_part = matrix_create(kalman->states, kalman->measurements, - gain_part_a); - - m_elem gain_sum_a[KALMAN_MAX_STATES * KALMAN_MAX_MEASUREMENTS] = - { }; - matrix_t gain_sum = matrix_create(kalman->states, kalman->measurements, - gain_sum_a); - - m_elem error_a[KALMAN_MAX_MEASUREMENTS * 1] = - { }; - matrix_t error = matrix_create(kalman->measurements, 1, error_a); - - m_elem measurement_estimate_a[KALMAN_MAX_MEASUREMENTS * 1] = - { }; - matrix_t measurement_estimate = matrix_create(kalman->measurements, 1, - measurement_estimate_a); - - m_elem x_update_a[KALMAN_MAX_STATES * 1] = - { }; - matrix_t x_update = matrix_create(kalman->states, 1, x_update_a); - - - - //x(:,i+1)=xapriori+(gainfactor*[M_50(:,1) M(:,2)]+(1-gainfactor)*M_start)*(z-C*xapriori); - - - //est=C*xapriori; - matrix_mult(kalman->c, kalman->x_apriori, measurement_estimate); - //error=(z-C*xapriori) = measurement-estimate - matrix_sub(measurement, measurement_estimate, error); - matrix_mult_element(error, mask, error); - - kalman->gainfactor = kalman->gainfactor * (1.0f - 1.0f - / kalman->gainfactorsteps) + 1.0f * 1.0f / kalman->gainfactorsteps; - - - - matrix_mult_scalar(kalman->gainfactor, kalman->gain, gain_part); - - matrix_mult_scalar(1.0f - kalman->gainfactor, kalman->gain_start, - gain_start_part); - - matrix_add(gain_start_part, gain_part, gain_sum); - - //gain*(z-C*xapriori) - matrix_mult(gain_sum, error, x_update); - - //xaposteriori = xapriori + update - - matrix_add(kalman->x_apriori, x_update, kalman->x_aposteriori); - - -// static int i=0; -// if(i++==4){ -// i=0; -// float_vect3 out_kal; -// out_kal.x = M(gain_sum,0,1); -//// out_kal_z.x = z_measurement[1]; -// out_kal.y = M(gain_sum,1,1); -// out_kal.z = M(gain_sum,2,1); -// debug_vect("out_kal", out_kal); -// } -} - -m_elem kalman_get_state(kalman_t *kalman, int state) -{ - return M(kalman->x_aposteriori, state, 0); -} diff --git a/apps/px4/attitude_estimator_bm/kalman.h b/apps/px4/attitude_estimator_bm/kalman.h deleted file mode 100644 index 0a6a18505..000000000 --- a/apps/px4/attitude_estimator_bm/kalman.h +++ /dev/null @@ -1,35 +0,0 @@ -/* - * kalman.h - * - * Created on: 01.12.2010 - * Author: Laurens Mackay - */ - -#ifndef KALMAN_H_ -#define KALMAN_H_ - -#include "matrix.h" - -#define KALMAN_MAX_STATES 12 -#define KALMAN_MAX_MEASUREMENTS 9 -typedef struct { - int states; - int measurements; - matrix_t a; - matrix_t c; - matrix_t gain_start; - matrix_t gain; - matrix_t x_apriori; - matrix_t x_aposteriori; - float gainfactor; - int gainfactorsteps; -} kalman_t; - -void kalman_init(kalman_t *kalman, int states, int measurements, m_elem a[], - m_elem c[], m_elem gain_start[], m_elem gain[], m_elem x_apriori[], - m_elem x_aposteriori[], int gainfactorsteps); -void kalman_predict(kalman_t *kalman); -void kalman_correct(kalman_t *kalman, m_elem measurement_a[], m_elem mask_a[]); -m_elem kalman_get_state(kalman_t *kalman, int state); - -#endif /* KALMAN_H_ */ diff --git a/apps/px4/attitude_estimator_bm/matrix.h b/apps/px4/attitude_estimator_bm/matrix.h deleted file mode 100644 index 613a2d081..000000000 --- a/apps/px4/attitude_estimator_bm/matrix.h +++ /dev/null @@ -1,156 +0,0 @@ -/* - * matrix.h - * - * Created on: 18.11.2010 - * Author: Laurens Mackay - */ - -#ifndef MATRIX_H_ -#define MATRIX_H_ - -typedef float m_elem; - -typedef struct { - int rows; - int cols; - m_elem *a; -} matrix_t; - -typedef struct { - float x; - float y; - float z; -} float_vect3; - -#define M(m,i,j) m.a[m.cols*i+j] - -///* This is the datatype used for the math and non-type specific ops. */ -// -//matrix_t matrix_create(const int rows, const int cols, m_elem * a); -///* matrix C = matrix A + matrix B , both of size m x n */ -//void matrix_add(const matrix_t a, const matrix_t b, matrix_t c); -// -///* matrix C = matrix A - matrix B , all of size m x n */ -//void matrix_sub(const matrix_t a, const matrix_t b, matrix_t c); -// -///* matrix C = matrix A x matrix B , A(a_rows x a_cols), B(a_cols x b_cols) */ -//void matrix_mult(const matrix_t a, const matrix_t b, matrix_t c); -// -//void matrix_mult_scalar(const float f, const matrix_t a, matrix_t c); -// -//void matrix_mult_element(const matrix_t a, const matrix_t b, matrix_t c); -// -///* matrix C = A*B'*/ -//void matrix_mult_trans(const matrix_t a, const matrix_t b, matrix_t c); - - -static inline matrix_t matrix_create(const int rows, const int cols, m_elem *a) -{ - matrix_t ret; - ret.rows = rows; - ret.cols = cols; - ret.a = a; - return ret; -} - -static inline void matrix_add(const matrix_t a, const matrix_t b, matrix_t c) -{ - if (a.rows != c.rows || a.cols != c.cols || b.rows != c.rows || b.cols - != c.cols) { - //debug_message_buffer("matrix_add: Dimension mismatch"); - } - - for (int i = 0; i < c.rows; i++) { - for (int j = 0; j < c.cols; j++) { - M(c, i, j) = M(a, i, j) + M(b, i, j); - } - - } -} - -static inline void matrix_sub(const matrix_t a, const matrix_t b, matrix_t c) -{ - if (a.rows != c.rows || a.cols != c.cols || b.rows != c.rows || b.cols - != c.cols) { - //debug_message_buffer("matrix_sub: Dimension mismatch"); - } - - for (int i = 0; i < c.rows; i++) { - for (int j = 0; j < c.cols; j++) { - M(c, i, j) = M(a, i, j) - M(b, i, j); - } - - } -} - -static inline void matrix_mult(const matrix_t a, const matrix_t b, matrix_t c) -{ - if (a.rows != c.rows || b.cols != c.cols || a.cols != b.rows) { - //debug_message_buffer("matrix_mult: Dimension mismatch"); - } - - for (int i = 0; i < a.rows; i++) { - for (int j = 0; j < b.cols; j++) { - M(c, i, j) = 0; - - for (int k = 0; k < a.cols; k++) { - M(c, i, j) += M(a, i, k) * M(b, k, j); - } - } - - } -} - -static inline void matrix_mult_trans(const matrix_t a, const matrix_t b, matrix_t c) -{ - - if (a.rows != c.rows || b.rows != c.cols || a.cols != b.cols) { - //debug_message_buffer("matrix_mult: Dimension mismatch"); - } - - for (int i = 0; i < a.rows; i++) { - for (int j = 0; j < b.cols; j++) { - M(c, i, j) = 0; - - for (int k = 0; k < a.cols; k++) { - M(c, i, j) += M(a, i, k) * M(b, j, k); - } - } - - } - -} - -static inline void matrix_mult_scalar(const float f, const matrix_t a, matrix_t c) -{ - if (a.rows != c.rows || a.cols != c.cols) { - //debug_message_buffer("matrix_mult_scalar: Dimension mismatch"); - } - - for (int i = 0; i < c.rows; i++) { - for (int j = 0; j < c.cols; j++) { - M(c, i, j) = f * M(a, i, j); - } - - } -} - - -static inline void matrix_mult_element(const matrix_t a, const matrix_t b, matrix_t c) -{ - if (a.rows != c.rows || a.cols != c.cols || b.rows != c.rows || b.cols - != c.cols) { - //debug_message_buffer("matrix_mult_element: Dimension mismatch"); - } - - for (int i = 0; i < c.rows; i++) { - for (int j = 0; j < c.cols; j++) { - M(c, i, j) = M(a, i, j) * M(b, i, j); - } - - } -} - - - -#endif /* MATRIX_H_ */ diff --git a/apps/px4/ground_estimator/Makefile b/apps/px4/ground_estimator/Makefile deleted file mode 100644 index b44d871c6..000000000 --- a/apps/px4/ground_estimator/Makefile +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# 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. -# -############################################################################ - -# -# Basic example application -# - -APPNAME = ground_estimator -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -include $(APPDIR)/mk/app.mk diff --git a/apps/px4/ground_estimator/ground_estimator.c b/apps/px4/ground_estimator/ground_estimator.c deleted file mode 100644 index ccf9ee3ec..000000000 --- a/apps/px4/ground_estimator/ground_estimator.c +++ /dev/null @@ -1,180 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Example User <mail@example.com> - * - * 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 ground_estimator.c - * ground_estimator application example for PX4 autopilot - */ - -#include <nuttx/config.h> -#include <unistd.h> -#include <stdio.h> -#include <poll.h> -#include <string.h> -#include <stdlib.h> - -#include <uORB/uORB.h> -#include <uORB/topics/sensor_combined.h> -#include <uORB/topics/debug_key_value.h> - - -static bool thread_should_exit = false; /**< ground_estimator exit flag */ -static bool thread_running = false; /**< ground_estimator status flag */ -static int ground_estimator_task; /**< Handle of ground_estimator task / thread */ - -/** - * ground_estimator management function. - */ -__EXPORT int ground_estimator_main(int argc, char *argv[]); - -/** - * Mainloop of ground_estimator. - */ -int ground_estimator_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -int ground_estimator_thread_main(int argc, char *argv[]) { - - printf("[ground_estimator] starting\n"); - - /* subscribe to raw data */ - int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); - - /* advertise debug value */ - struct debug_key_value_s dbg = { .key = "posx", .value = 0.0f }; - orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg); - - float position[3] = {}; - float velocity[3] = {}; - - uint64_t last_time = 0; - - struct pollfd fds[] = { - { .fd = sub_raw, .events = POLLIN }, - }; - - while (!thread_should_exit) { - - /* wait for sensor update */ - int ret = poll(fds, 1, 1000); - - if (ret < 0) { - /* XXX this is seriously bad - should be an emergency */ - } else if (ret == 0) { - /* XXX this means no sensor data - should be critical or emergency */ - printf("[ground estimator bm] WARNING: Not getting sensor data - sensor app running?\n"); - } else { - struct sensor_combined_s s; - orb_copy(ORB_ID(sensor_combined), sub_raw, &s); - - float dt = ((float)(s.timestamp - last_time)) / 1000000.0f; - - /* Integration */ - position[0] += velocity[0] * dt; - position[1] += velocity[1] * dt; - position[2] += velocity[2] * dt; - - velocity[0] += velocity[0] * 0.01f + 0.99f * s.accelerometer_m_s2[0] * dt; - velocity[1] += velocity[1] * 0.01f + 0.99f * s.accelerometer_m_s2[1] * dt; - velocity[2] += velocity[2] * 0.01f + 0.99f * s.accelerometer_m_s2[2] * dt; - - dbg.value = position[0]; - - orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); - } - - } - - printf("[ground_estimator] exiting.\n"); - - return 0; -} - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: ground_estimator {start|stop|status} [-p <additional params>]\n\n"); - exit(1); -} - -/** - * The ground_estimator 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 ground_estimator_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - printf("ground_estimator already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - ground_estimator_task = task_create("ground_estimator", SCHED_PRIORITY_DEFAULT, 4096, ground_estimator_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); - thread_running = true; - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - printf("\tground_estimator is running\n"); - } else { - printf("\tground_estimator not started\n"); - } - exit(0); - } - - usage("unrecognized command"); - exit(1); -} |