diff options
-rw-r--r-- | ROMFS/px4fmu_common/init.d/rc.mc_apps | 2 | ||||
-rw-r--r-- | makefiles/config_px4fmu-v2_default.mk | 1 | ||||
-rw-r--r-- | src/lib/mathlib/math/Matrix.hpp | 18 | ||||
-rw-r--r-- | src/lib/mathlib/math/Quaternion.hpp | 106 | ||||
-rw-r--r-- | src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp | 463 | ||||
-rw-r--r-- | src/modules/attitude_estimator_q/attitude_estimator_q_params.c | 48 | ||||
-rw-r--r-- | src/modules/attitude_estimator_q/module.mk | 43 |
7 files changed, 656 insertions, 25 deletions
diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 6517e026a..a28ff1bbe 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -8,7 +8,7 @@ # INAV, which defaults to 0 now. if param compare INAV_ENABLED 1 then - attitude_estimator_ekf start + attitude_estimator_q start position_estimator_inav start else ekf_att_pos_estimator start diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 324e12696..ccca43a54 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -78,6 +78,7 @@ MODULES += modules/land_detector # Estimation modules (EKF/ SO3 / other filters) # MODULES += modules/attitude_estimator_ekf +MODULES += modules/attitude_estimator_q MODULES += modules/ekf_att_pos_estimator MODULES += modules/position_estimator_inav diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index f6f4fc5ea..2a7b61238 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -136,6 +136,24 @@ public: #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 */ float &operator()(const unsigned int row, const unsigned int col) { diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index d28966fca..0603f0049 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -94,6 +94,19 @@ public: } /** + * 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 */ const Quaternion derivative(const Vector<3> &w) { @@ -109,39 +122,73 @@ public: } /** - * imaginary part of quaternion + * conjugate */ - Vector<3> imag(void) { - return Vector<3>(&data[1]); + Quaternion conjugated() const { + return Quaternion(data[0], -data[1], -data[2], -data[3]); } /** - * inverse of quaternion + * inversed */ - 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; + 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) + ); + } /** - * 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; + * 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 + */ + Vector<3> imag(void) { + return Vector<3>(&data[1]); } /** @@ -165,6 +212,17 @@ public: } /** + * create Euler angles vector from the quaternion + */ + Vector<3> to_euler() const { + return Vector<3>( + atan2(2.0f * (data[0] * data[1] + data[2] * data[3]), 1.0f - 2.0f * (data[1] * data[1] + data[2] * data[2])), + asin(2.0f * (data[0] * data[2] - data[3] * data[1])), + atan2(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 * Reference: Shoemake, Quaternions, http://www.cs.ucr.edu/~vbz/resources/quatut.pdf */ diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp new file mode 100644 index 000000000..7b5c8b7dc --- /dev/null +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -0,0 +1,463 @@ +/**************************************************************************** + * + * 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_gps_position.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 acc_comp; + } _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; + + Vector<3> _gyro; + Vector<3> _accel; + Vector<3> _mag; + bool _acc_comp = false; + + 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.acc_comp = param_find("ATT_ACC_COMP"); +} + +/** + * 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 (_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, ¶m_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 acc_comp_int; + param_get(_params_handles.acc_comp, &acc_comp_int); + _acc_comp = acc_comp_int != 0; + } +} + +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); + _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; +} diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c new file mode 100644 index 000000000..d674c3ad6 --- /dev/null +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c @@ -0,0 +1,48 @@ +/**************************************************************************** + * + * 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_ACC_COMP, 2); ///< acceleration compensation diff --git a/src/modules/attitude_estimator_q/module.mk b/src/modules/attitude_estimator_q/module.mk new file mode 100644 index 000000000..b3688e4a9 --- /dev/null +++ b/src/modules/attitude_estimator_q/module.mk @@ -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 |