mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-25 08:33:34 +08:00
Deleted old cruft
This commit is contained in:
Lorenz Meier
@@ -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
|
||||
@@ -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);
|
||||
|
||||
}
|
||||
@@ -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_ */
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
@@ -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_ */
|
||||
@@ -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_ */
|
||||
@@ -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
|
||||
@@ -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);
|
||||
}
|
||||
Reference in New Issue
Block a user