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

Merge pull request #2013 from PX4/attitude_estimator_q

Browse Source 
attitude_estimator_q added
This commit is contained in:
Lorenz Meier
2015-05-05 20:29:51 +02:00
parent 8f606cd491 4ce7e2acc1
commit 6c859245e2
8 changed files with 685 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/rc.mc_apps
+1 -1
View File
@@ -8,7 +8,7 @@
# INAV, which defaults to 0 now. # INAV, which defaults to 0 now.
if param compare INAV_ENABLED 1 if param compare INAV_ENABLED 1
then then
attitude_estimator_ekf start attitude_estimator_q start
position_estimator_inav start position_estimator_inav start
else else
ekf_att_pos_estimator start ekf_att_pos_estimator start
makefiles/config_px4fmu-v2_default.mk
+1
View File
@@ -77,6 +77,7 @@ MODULES += modules/land_detector
# Estimation modules (EKF/ SO3 / other filters) # Estimation modules (EKF/ SO3 / other filters)
# #
MODULES += modules/attitude_estimator_ekf MODULES += modules/attitude_estimator_ekf
MODULES += modules/attitude_estimator_q
MODULES += modules/ekf_att_pos_estimator MODULES += modules/ekf_att_pos_estimator
MODULES += modules/position_estimator_inav MODULES += modules/position_estimator_inav
src/lib/mathlib/math/Matrix.hpp
+18
View File
@@ -135,6 +135,24 @@ public:
} }
#endif #endif
/**
* set row from vector
*/
void set_row(unsigned int row, const Vector<N> v) {
for (unsigned i = 0; i < N; i++) {
data[row][i] = v.data[i];
}
}
/**
* set column from vector
*/
void set_col(unsigned int col, const Vector<M> v) {
for (unsigned i = 0; i < M; i++) {
data[i][col] = v.data[i];
}
}
/** /**
* access by index * access by index
*/ */
src/lib/mathlib/math/Quaternion.hpp
+87 -29
View File
@@ -93,6 +93,19 @@ public:
data[0] * q.data[3] + data[1] * q.data[2] - data[2] * q.data[1] + data[3] * q.data[0]); data[0] * q.data[3] + data[1] * q.data[2] - data[2] * q.data[1] + data[3] * q.data[0]);
} }
/**
* division
*/
Quaternion operator /(const Quaternion &q) const {
float norm = q.length_squared();
return Quaternion(
( data[0] * q.data[0] + data[1] * q.data[1] + data[2] * q.data[2] + data[3] * q.data[3]) / norm,
(- data[0] * q.data[1] + data[1] * q.data[0] - data[2] * q.data[3] + data[3] * q.data[2]) / norm,
(- data[0] * q.data[2] + data[1] * q.data[3] + data[2] * q.data[0] - data[3] * q.data[1]) / norm,
(- data[0] * q.data[3] - data[1] * q.data[2] + data[2] * q.data[1] + data[3] * q.data[0]) / norm
);
}
/** /**
* derivative * derivative
*/ */
@@ -108,6 +121,69 @@ public:
return Q * v * 0.5f; return Q * v * 0.5f;
} }
/**
* conjugate
*/
Quaternion conjugated() const {
return Quaternion(data[0], -data[1], -data[2], -data[3]);
}
/**
* inversed
*/
Quaternion inversed() const {
float norm = length_squared();
return Quaternion(data[0] / norm, -data[1] / norm, -data[2] / norm, -data[3] / norm);
}
/**
* conjugation
*/
Vector<3> conjugate(const Vector<3> &v) const {
float q0q0 = data[0] * data[0];
float q1q1 = data[1] * data[1];
float q2q2 = data[2] * data[2];
float q3q3 = data[3] * data[3];
return Vector<3>(
v.data[0] * (q0q0 + q1q1 - q2q2 - q3q3) +
v.data[1] * 2.0f * (data[1] * data[2] - data[0] * data[3]) +
v.data[2] * 2.0f * (data[0] * data[2] + data[1] * data[3]),
v.data[0] * 2.0f * (data[1] * data[2] + data[0] * data[3]) +
v.data[1] * (q0q0 - q1q1 + q2q2 - q3q3) +
v.data[2] * 2.0f * (data[2] * data[3] - data[0] * data[1]),
v.data[0] * 2.0f * (data[1] * data[3] - data[0] * data[2]) +
v.data[1] * 2.0f * (data[0] * data[1] + data[2] * data[3]) +
v.data[2] * (q0q0 - q1q1 - q2q2 + q3q3)
);
}
/**
* conjugation with inversed quaternion
*/
Vector<3> conjugate_inversed(const Vector<3> &v) const {
float q0q0 = data[0] * data[0];
float q1q1 = data[1] * data[1];
float q2q2 = data[2] * data[2];
float q3q3 = data[3] * data[3];
return Vector<3>(
v.data[0] * (q0q0 + q1q1 - q2q2 - q3q3) +
v.data[1] * 2.0f * (data[1] * data[2] + data[0] * data[3]) +
v.data[2] * 2.0f * (data[1] * data[3] - data[0] * data[2]),
v.data[0] * 2.0f * (data[1] * data[2] - data[0] * data[3]) +
v.data[1] * (q0q0 - q1q1 + q2q2 - q3q3) +
v.data[2] * 2.0f * (data[2] * data[3] + data[0] * data[1]),
v.data[0] * 2.0f * (data[1] * data[3] + data[0] * data[2]) +
v.data[1] * 2.0f * (data[2] * data[3] - data[0] * data[1]) +
v.data[2] * (q0q0 - q1q1 - q2q2 + q3q3)
);
}
/** /**
* imaginary part of quaternion * imaginary part of quaternion
*/ */
@@ -115,35 +191,6 @@ public:
return Vector<3>(&data[1]); return Vector<3>(&data[1]);
} }
/**
* inverse of quaternion
*/
math::Quaternion inverse() {
Quaternion res;
memcpy(res.data,data,sizeof(res.data));
res.data[1] = -res.data[1];
res.data[2] = -res.data[2];
res.data[3] = -res.data[3];
return res;
}
/**
* rotate vector by quaternion
*/
Vector<3> rotate(const Vector<3> &w) {
Quaternion q_w; // extend vector to quaternion
Quaternion q = {data[0],data[1],data[2],data[3]};
Quaternion q_rotated; // quaternion representation of rotated vector
q_w(0) = 0;
q_w(1) = w.data[0];
q_w(2) = w.data[1];
q_w(3) = w.data[2];
q_rotated = q*q_w*q.inverse();
Vector<3> res = {q_rotated.data[1],q_rotated.data[2],q_rotated.data[3]};
return res;
}
/** /**
* set quaternion to rotation defined by euler angles * set quaternion to rotation defined by euler angles
*/ */
@@ -164,6 +211,17 @@ public:
data[3] = static_cast<float>(cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2); data[3] = static_cast<float>(cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2);
} }
/**
* create Euler angles vector from the quaternion
*/
Vector<3> to_euler() const {
return Vector<3>(
atan2f(2.0f * (data[0] * data[1] + data[2] * data[3]), 1.0f - 2.0f * (data[1] * data[1] + data[2] * data[2])),
asinf(2.0f * (data[0] * data[2] - data[3] * data[1])),
atan2f(2.0f * (data[0] * data[3] + data[1] * data[2]), 1.0f - 2.0f * (data[2] * data[2] + data[3] * data[3]))
);
}
/** /**
* set quaternion to rotation by DCM * set quaternion to rotation by DCM
* Reference: Shoemake, Quaternions, http://www.cs.ucr.edu/~vbz/resources/quatut.pdf * Reference: Shoemake, Quaternions, http://www.cs.ucr.edu/~vbz/resources/quatut.pdf
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+479
View File
@@ -0,0 +1,479 @@
/****************************************************************************
*
* Copyright (c) 2015 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.
*
****************************************************************************/
/*
* @file attitude_estimator_q_main.cpp
*
* Attitude estimator (quaternion based)
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#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_control_mode.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_hrt.h>
#include <lib/mathlib/mathlib.h>
#include <lib/geo/geo.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
extern "C" __EXPORT int attitude_estimator_q_main(int argc, char *argv[]);
using math::Vector;
using math::Matrix;
using math::Quaternion;
class AttitudeEstimatorQ;
namespace attitude_estimator_q {
AttitudeEstimatorQ *instance;
}
class AttitudeEstimatorQ {
public:
/**
* Constructor
*/
AttitudeEstimatorQ();
/**
* Destructor, also kills task.
*/
~AttitudeEstimatorQ();
/**
* Start task.
*
* @return OK on success.
*/
int start();
static void task_main_trampoline(int argc, char *argv[]);
void task_main();
private:
static constexpr float _dt_max = 0.02;
bool _task_should_exit = false; /**< if true, task should exit */
int _control_task = -1; /**< task handle for task */
int _sensors_sub = -1;
int _params_sub = -1;
int _global_pos_sub = -1;
orb_advert_t _att_pub = -1;
struct {
param_t w_acc;
param_t w_mag;
param_t w_gyro_bias;
param_t mag_decl;
param_t mag_decl_auto;
param_t acc_comp;
param_t bias_max;
} _params_handles; /**< handles for interesting parameters */
float _w_accel = 0.0f;
float _w_mag = 0.0f;
float _w_gyro_bias = 0.0f;
float _mag_decl = 0.0f;
bool _mag_decl_auto = false;
bool _acc_comp = false;
float _bias_max = 0.0f;
Vector<3> _gyro;
Vector<3> _accel;
Vector<3> _mag;
Quaternion _q;
Vector<3> _rates;
Vector<3> _gyro_bias;
vehicle_global_position_s _gpos = {};
Vector<3> _vel_prev;
Vector<3> _pos_acc;
hrt_abstime _vel_prev_t = 0;
bool _inited = false;
perf_counter_t _update_perf;
perf_counter_t _loop_perf;
void update_parameters(bool force);
int update_subscriptions();
void init();
void update(float dt);
};
AttitudeEstimatorQ::AttitudeEstimatorQ() {
_params_handles.w_acc = param_find("ATT_W_ACC");
_params_handles.w_mag = param_find("ATT_W_MAG");
_params_handles.w_gyro_bias = param_find("ATT_W_GYRO_BIAS");
_params_handles.mag_decl = param_find("ATT_MAG_DECL");
_params_handles.mag_decl_auto = param_find("ATT_MAG_DECL_A");
_params_handles.acc_comp = param_find("ATT_ACC_COMP");
_params_handles.bias_max = param_find("ATT_BIAS_MAX");
}
/**
* Destructor, also kills task.
*/
AttitudeEstimatorQ::~AttitudeEstimatorQ() {
if (_control_task != -1) {
/* task wakes up every 100ms or so at the longest */
_task_should_exit = true;
/* wait for a second for the task to quit at our request */
unsigned i = 0;
do {
/* wait 20ms */
usleep(20000);
/* if we have given up, kill it */
if (++i > 50) {
task_delete(_control_task);
break;
}
} while (_control_task != -1);
}
attitude_estimator_q::instance = nullptr;
}
int AttitudeEstimatorQ::start() {
ASSERT(_control_task == -1);
/* start the task */
_control_task = task_spawn_cmd("attitude_estimator_q",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2500,
(main_t)&AttitudeEstimatorQ::task_main_trampoline,
nullptr);
if (_control_task < 0) {
warn("task start failed");
return -errno;
}
return OK;
}
void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[]) {
attitude_estimator_q::instance->task_main();
}
void AttitudeEstimatorQ::task_main() {
warnx("started");
_sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
update_parameters(true);
hrt_abstime last_time = 0;
struct pollfd fds[1];
fds[0].fd = _sensors_sub;
fds[0].events = POLLIN;
while (!_task_should_exit) {
int ret = poll(fds, 1, 1000);
if (ret < 0) {
// Poll error, sleep and try again
usleep(10000);
continue;
} else if (ret == 0) {
// Poll timeout, do nothing
continue;
}
update_parameters(false);
// Update sensors
sensor_combined_s sensors;
if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) {
_gyro.set(sensors.gyro_rad_s);
_accel.set(sensors.accelerometer_m_s2);
_mag.set(sensors.magnetometer_ga);
}
bool gpos_updated;
orb_check(_global_pos_sub, &gpos_updated);
if (gpos_updated) {
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_gpos);
if (_mag_decl_auto && _gpos.eph < 20.0f && hrt_elapsed_time(&_gpos.timestamp) < 1000000) {
/* set magnetic declination automatically */
_mag_decl = math::radians(get_mag_declination(_gpos.lat, _gpos.lon));
}
}
if (_acc_comp && _gpos.timestamp != 0 && hrt_absolute_time() < _gpos.timestamp + 20000 && _gpos.eph < 5.0f) {
/* position data is actual */
if (gpos_updated) {
Vector<3> vel(_gpos.vel_n, _gpos.vel_e, _gpos.vel_d);
/* velocity updated */
if (_vel_prev_t != 0 && _gpos.timestamp != _vel_prev_t) {
float vel_dt = (_gpos.timestamp - _vel_prev_t) / 1000000.0f;
/* calculate acceleration in body frame */
_pos_acc = _q.conjugate_inversed((vel - _vel_prev) / vel_dt);
}
_vel_prev_t = _gpos.timestamp;
_vel_prev = vel;
}
} else {
/* position data is outdated, reset acceleration */
_pos_acc.zero();
_vel_prev.zero();
_vel_prev_t = 0;
}
// Time from previous iteration
uint64_t now = hrt_absolute_time();
float dt = (last_time > 0) ? ((now - last_time) / 1000000.0f) : 0.0f;
last_time = now;
if (dt > _dt_max) {
dt = _dt_max;
}
update(dt);
Vector<3> euler = _q.to_euler();
struct vehicle_attitude_s att = {};
att.timestamp = sensors.timestamp;
att.roll = euler(0);
att.pitch = euler(1);
att.yaw = euler(2);
att.rollspeed = _rates(0);
att.pitchspeed = _rates(1);
att.yawspeed = _rates(2);
for (int i = 0; i < 3; i++) {
att.g_comp[i] = _accel(i) - _pos_acc(i);
}
/* copy offsets */
memcpy(&att.rate_offsets, _gyro_bias.data, sizeof(att.rate_offsets));
Matrix<3, 3> R = _q.to_dcm();
/* copy rotation matrix */
memcpy(&att.R[0], R.data, sizeof(att.R));
att.R_valid = true;
if (_att_pub < 0) {
_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
} else {
orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
}
}
}
void AttitudeEstimatorQ::update_parameters(bool force) {
bool updated = force;
if (!updated) {
orb_check(_params_sub, &updated);
}
if (updated) {
parameter_update_s param_update;
orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
param_get(_params_handles.w_acc, &_w_accel);
param_get(_params_handles.w_mag, &_w_mag);
param_get(_params_handles.w_gyro_bias, &_w_gyro_bias);
float mag_decl_deg = 0.0f;
param_get(_params_handles.mag_decl, &mag_decl_deg);
_mag_decl = math::radians(mag_decl_deg);
int32_t mag_decl_auto_int;
param_get(_params_handles.mag_decl_auto, &mag_decl_auto_int);
_mag_decl_auto = mag_decl_auto_int != 0;
int32_t acc_comp_int;
param_get(_params_handles.acc_comp, &acc_comp_int);
_acc_comp = acc_comp_int != 0;
param_get(_params_handles.bias_max, &_bias_max);
}
}
void AttitudeEstimatorQ::init() {
// Rotation matrix can be easily constructed from acceleration and mag field vectors
// 'k' is Earth Z axis (Down) unit vector in body frame
Vector<3> k = -_accel;
k.normalize();
// 'i' is Earth X axis (North) unit vector in body frame, orthogonal with 'k'
Vector<3> i = (_mag - k * (_mag * k));
i.normalize();
// 'j' is Earth Y axis (East) unit vector in body frame, orthogonal with 'k' and 'i'
Vector<3> j = k % i;
// Fill rotation matrix
Matrix<3, 3> R;
R.set_row(0, i);
R.set_row(1, j);
R.set_row(2, k);
// Convert to quaternion
_q.from_dcm(R);
}
void AttitudeEstimatorQ::update(float dt) {
if (!_inited) {
init();
_inited = true;
}
// Angular rate of correction
Vector<3> corr;
// Magnetometer correction
// Project mag field vector to global frame and extract XY component
Vector<3> mag_earth = _q.conjugate(_mag);
float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
// Project magnetometer correction to body frame
corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag;
// Accelerometer correction
// Project 'k' unit vector of earth frame to body frame
// Vector<3> k = _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, 1.0f));
// Optimized version with dropped zeros
Vector<3> k(
2.0f * (_q(1) * _q(3) - _q(0) * _q(2)),
2.0f * (_q(2) * _q(3) + _q(0) * _q(1)),
(_q(0) * _q(0) - _q(1) * _q(1) - _q(2) * _q(2) + _q(3) * _q(3))
);
corr += (k % (_accel - _pos_acc).normalized()) * _w_accel;
// Gyro bias estimation
_gyro_bias += corr * (_w_gyro_bias * dt);
for (int i = 0; i < 3; i++) {
_gyro_bias(i) = math::constrain(_gyro_bias(i), -_bias_max, _bias_max);
}
_rates = _gyro + _gyro_bias;
// Feed forward gyro
corr += _rates;
// Apply correction to state
_q += _q.derivative(corr) * dt;
// Normalize quaternion
_q.normalize(); // TODO! NaN protection???
}
int attitude_estimator_q_main(int argc, char *argv[]) {
if (argc < 1) {
errx(1, "usage: attitude_estimator_q {start|stop|status}");
}
if (!strcmp(argv[1], "start")) {
if (attitude_estimator_q::instance != nullptr) {
errx(1, "already running");
}
attitude_estimator_q::instance = new AttitudeEstimatorQ;
if (attitude_estimator_q::instance == nullptr) {
errx(1, "alloc failed");
}
if (OK != attitude_estimator_q::instance->start()) {
delete attitude_estimator_q::instance;
attitude_estimator_q::instance = nullptr;
err(1, "start failed");
}
exit(0);
}
if (!strcmp(argv[1], "stop")) {
if (attitude_estimator_q::instance == nullptr) {
errx(1, "not running");
}
delete attitude_estimator_q::instance;
attitude_estimator_q::instance = nullptr;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (attitude_estimator_q::instance) {
errx(0, "running");
} else {
errx(1, "not running");
}
}
warnx("unrecognized command");
return 1;
}
src/modules/attitude_estimator_q/attitude_estimator_q_params.c
+50
View File
@@ -0,0 +1,50 @@
/****************************************************************************
*
* Copyright (c) 2015 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.
*
****************************************************************************/
/*
* @file attitude_estimator_q_params.c
*
* Parameters for attitude estimator (quaternion based)
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <systemlib/param/param.h>
PARAM_DEFINE_FLOAT(ATT_W_ACC, 0.2f);
PARAM_DEFINE_FLOAT(ATT_W_MAG, 0.1f);
PARAM_DEFINE_FLOAT(ATT_W_GYRO_BIAS, 0.1f);
PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); ///< magnetic declination, in degrees
PARAM_DEFINE_INT32(ATT_MAG_DECL_A, 1); ///< automatic GPS based magnetic declination
PARAM_DEFINE_INT32(ATT_ACC_COMP, 2); ///< acceleration compensation
PARAM_DEFINE_FLOAT(ATT_BIAS_MAX, 0.05f); ///< gyro bias limit, rad/s
src/modules/attitude_estimator_q/module.mk
+43
View File
@@ -0,0 +1,43 @@
############################################################################
#
# Copyright (c) 2015 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.
#
############################################################################
#
# Attitude estimator (quaternion based)
#
MODULE_COMMAND = attitude_estimator_q
SRCS = attitude_estimator_q_main.cpp \
attitude_estimator_q_params.c
MODULE_STACKSIZE = 1200
src/systemcmds/tests/test_mathlib.cpp
+6 -6
View File
@@ -300,7 +300,7 @@ int test_mathlib(int argc, char *argv[])
R.from_euler(roll, pitch, yaw); R.from_euler(roll, pitch, yaw);
q.from_euler(roll, pitch, yaw); q.from_euler(roll, pitch, yaw);
vector_r = R * vector; vector_r = R * vector;
vector_q = q.rotate(vector); vector_q = q.conjugate(vector);
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
if (fabsf(vector_r(i) - vector_q(i)) > tol) { if (fabsf(vector_r(i) - vector_q(i)) > tol) {
@@ -315,7 +315,7 @@ int test_mathlib(int argc, char *argv[])
// test some values calculated with matlab // test some values calculated with matlab
tol = 0.0001f; tol = 0.0001f;
q.from_euler(M_PI_2_F, 0.0f, 0.0f); q.from_euler(M_PI_2_F, 0.0f, 0.0f);
vector_q = q.rotate(vector); vector_q = q.conjugate(vector);
Vector<3> vector_true = {1.00f, -1.00f, 1.00f}; Vector<3> vector_true = {1.00f, -1.00f, 1.00f};
for (unsigned i = 0; i < 3; i++) { for (unsigned i = 0; i < 3; i++) {
@@ -326,7 +326,7 @@ int test_mathlib(int argc, char *argv[])
} }
q.from_euler(0.3f, 0.2f, 0.1f); q.from_euler(0.3f, 0.2f, 0.1f);
vector_q = q.rotate(vector); vector_q = q.conjugate(vector);
vector_true = {1.1566, 0.7792, 1.0273}; vector_true = {1.1566, 0.7792, 1.0273};
for (unsigned i = 0; i < 3; i++) { for (unsigned i = 0; i < 3; i++) {
@@ -337,7 +337,7 @@ int test_mathlib(int argc, char *argv[])
} }
q.from_euler(-1.5f, -0.2f, 0.5f); q.from_euler(-1.5f, -0.2f, 0.5f);
vector_q = q.rotate(vector); vector_q = q.conjugate(vector);
vector_true = {0.5095, 1.4956, -0.7096}; vector_true = {0.5095, 1.4956, -0.7096};
for (unsigned i = 0; i < 3; i++) { for (unsigned i = 0; i < 3; i++) {
@@ -348,7 +348,7 @@ int test_mathlib(int argc, char *argv[])
} }
q.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f); q.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f);
vector_q = q.rotate(vector); vector_q = q.conjugate(vector);
vector_true = { -1.3660, 0.3660, 1.0000}; vector_true = { -1.3660, 0.3660, 1.0000};
for (unsigned i = 0; i < 3; i++) { for (unsigned i = 0; i < 3; i++) {
@@ -359,4 +359,4 @@ int test_mathlib(int argc, char *argv[])
} }
} }
return rc; return rc;
} }