/****************************************************************************
*
* 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;
}