diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
index 8136e4e4c2b9da07c9935021358a63679be87082..b65c29bd488cad0be7f19e1be09db5f008cb1250 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -39,38 +39,21 @@
* @author Anton Babushkin <anton.babushkin@me.com>
*/
+#include <drivers/drv_hrt.h>
+#include <lib/geo/geo.h>
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
+#include <mathlib/mathlib.h>
#include <px4_config.h>
#include <px4_posix.h>
#include <px4_tasks.h>
-#include <unistd.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <stdbool.h>
-#include <poll.h>
-#include <fcntl.h>
-#include <float.h>
-#include <errno.h>
-#include <limits.h>
-#include <math.h>
-#include <uORB/uORB.h>
+#include <systemlib/err.h>
+#include <systemlib/param/param.h>
+#include <systemlib/perf_counter.h>
+#include <uORB/topics/att_pos_mocap.h>
+#include <uORB/topics/parameter_update.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/att_pos_mocap.h>
-#include <uORB/topics/airspeed.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/estimator_status.h>
-#include <drivers/drv_hrt.h>
-
-#include <mathlib/mathlib.h>
-#include <mathlib/math/filter/LowPassFilter2p.hpp>
-#include <lib/geo/geo.h>
-
-#include <systemlib/param/param.h>
-#include <systemlib/perf_counter.h>
-#include <systemlib/err.h>
-#include <systemlib/mavlink_log.h>
extern "C" __EXPORT int attitude_estimator_q_main(int argc, char *argv[]);
@@ -83,7 +66,7 @@ class AttitudeEstimatorQ;
namespace attitude_estimator_q
{
AttitudeEstimatorQ *instance;
-}
+} // namespace attitude_estimator_q
class AttitudeEstimatorQ
@@ -110,21 +93,20 @@ public:
void task_main();
- void print();
-
private:
- static constexpr float _dt_max = 0.02;
+ const float _dt_min = 0.00001f;
+ const float _dt_max = 0.02f;
+
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 _sensors_sub = -1;
+ int _global_pos_sub = -1;
int _vision_sub = -1;
int _mocap_sub = -1;
- int _airspeed_sub = -1;
- int _global_pos_sub = -1;
+
orb_advert_t _att_pub = nullptr;
- orb_advert_t _est_state_pub = nullptr;
struct {
param_t w_acc;
@@ -136,8 +118,7 @@ private:
param_t acc_comp;
param_t bias_max;
param_t ext_hdg_mode;
- param_t airspeed_disabled;
- } _params_handles; /**< handles for interesting parameters */
+ } _params_handles{}; /**< handles for interesting parameters */
float _w_accel = 0.0f;
float _w_mag = 0.0f;
@@ -147,42 +128,28 @@ private:
bool _mag_decl_auto = false;
bool _acc_comp = false;
float _bias_max = 0.0f;
- int _ext_hdg_mode = 0;
- int _airspeed_disabled = 0;
+ int32_t _ext_hdg_mode = 0;
Vector<3> _gyro;
Vector<3> _accel;
Vector<3> _mag;
- vehicle_attitude_s _vision = {};
Vector<3> _vision_hdg;
-
- att_pos_mocap_s _mocap = {};
Vector<3> _mocap_hdg;
- airspeed_s _airspeed = {};
-
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;
- /* Low pass filter for accel/gyro */
- math::LowPassFilter2p _lp_accel_x;
- math::LowPassFilter2p _lp_accel_y;
- math::LowPassFilter2p _lp_accel_z;
-
- hrt_abstime _vel_prev_t = 0;
+ Vector<3> _pos_acc;
bool _inited = false;
bool _data_good = false;
bool _ext_hdg_good = false;
- orb_advert_t _mavlink_log_pub = nullptr;
-
void update_parameters(bool force);
int update_subscriptions();
@@ -196,12 +163,7 @@ private:
};
-AttitudeEstimatorQ::AttitudeEstimatorQ() :
- _vel_prev(0, 0, 0),
- _pos_acc(0, 0, 0),
- _lp_accel_x(250.0f, 30.0f),
- _lp_accel_y(250.0f, 30.0f),
- _lp_accel_z(250.0f, 30.0f)
+AttitudeEstimatorQ::AttitudeEstimatorQ()
{
_params_handles.w_acc = param_find("ATT_W_ACC");
_params_handles.w_mag = param_find("ATT_W_MAG");
@@ -212,7 +174,23 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() :
_params_handles.acc_comp = param_find("ATT_ACC_COMP");
_params_handles.bias_max = param_find("ATT_BIAS_MAX");
_params_handles.ext_hdg_mode = param_find("ATT_EXT_HDG_M");
- _params_handles.airspeed_disabled = param_find("FW_ARSP_MODE");
+
+ _vel_prev.zero();
+ _pos_acc.zero();
+
+ _gyro.zero();
+ _accel.zero();
+ _mag.zero();
+
+ _vision_hdg.zero();
+ _mocap_hdg.zero();
+
+ _q.zero();
+ _rates.zero();
+ _gyro_bias.zero();
+
+ _vel_prev.zero();
+ _pos_acc.zero();
}
/**
@@ -249,7 +227,7 @@ int AttitudeEstimatorQ::start()
/* start the task */
_control_task = px4_task_spawn_cmd("attitude_estimator_q",
SCHED_DEFAULT,
- SCHED_PRIORITY_MAX - 5,
+ SCHED_PRIORITY_ESTIMATOR,
2000,
(px4_main_t)&AttitudeEstimatorQ::task_main_trampoline,
nullptr);
@@ -262,10 +240,6 @@ int AttitudeEstimatorQ::start()
return OK;
}
-void AttitudeEstimatorQ::print()
-{
-}
-
void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[])
{
attitude_estimator_q::instance->task_main();
@@ -281,12 +255,8 @@ void AttitudeEstimatorQ::task_main()
#endif
_sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
-
_vision_sub = orb_subscribe(ORB_ID(vehicle_vision_attitude));
_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
-
- _airspeed_sub = orb_subscribe(ORB_ID(airspeed));
-
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
@@ -304,12 +274,12 @@ void AttitudeEstimatorQ::task_main()
if (ret < 0) {
// Poll error, sleep and try again
usleep(10000);
- PX4_WARN("Q POLL ERROR");
+ PX4_WARN("POLL ERROR");
continue;
} else if (ret == 0) {
// Poll timeout, do nothing
- PX4_WARN("Q POLL TIMEOUT");
+ PX4_WARN("POLL TIMEOUT");
continue;
}
@@ -318,7 +288,7 @@ void AttitudeEstimatorQ::task_main()
// Update sensors
sensor_combined_s sensors;
- if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) {
+ if (orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors) == PX4_OK) {
// Feed validator with recent sensor data
if (sensors.timestamp > 0) {
@@ -328,13 +298,12 @@ void AttitudeEstimatorQ::task_main()
}
if (sensors.accelerometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
- // Filter accel signal since it is not filtered in the drivers.
- _accel(0) = _lp_accel_x.apply(sensors.accelerometer_m_s2[0]);
- _accel(1) = _lp_accel_y.apply(sensors.accelerometer_m_s2[1]);
- _accel(2) = _lp_accel_z.apply(sensors.accelerometer_m_s2[2]);
+ _accel(0) = sensors.accelerometer_m_s2[0];
+ _accel(1) = sensors.accelerometer_m_s2[1];
+ _accel(2) = sensors.accelerometer_m_s2[2];
if (_accel.length() < 0.01f) {
- PX4_DEBUG("WARNING: degenerate accel!");
+ PX4_ERR("WARNING: degenerate accel!");
continue;
}
}
@@ -345,7 +314,7 @@ void AttitudeEstimatorQ::task_main()
_mag(2) = sensors.magnetometer_ga[2];
if (_mag.length() < 0.01f) {
- PX4_DEBUG("WARNING: degenerate mag!");
+ PX4_ERR("WARNING: degenerate mag!");
continue;
}
}
@@ -357,125 +326,108 @@ void AttitudeEstimatorQ::task_main()
bool vision_updated = false;
orb_check(_vision_sub, &vision_updated);
- bool mocap_updated = false;
- orb_check(_mocap_sub, &mocap_updated);
-
if (vision_updated) {
- orb_copy(ORB_ID(vehicle_vision_attitude), _vision_sub, &_vision);
- math::Quaternion q(_vision.q);
+ vehicle_attitude_s vision;
- math::Matrix<3, 3> Rvis = q.to_dcm();
- math::Vector<3> v(1.0f, 0.0f, 0.4f);
+ if (orb_copy(ORB_ID(vehicle_vision_attitude), _vision_sub, &vision) == PX4_OK) {
+ math::Quaternion q(vision.q);
- // Rvis is Rwr (robot respect to world) while v is respect to world.
- // Hence Rvis must be transposed having (Rwr)' * Vw
- // Rrw * Vw = vn. This way we have consistency
- _vision_hdg = Rvis.transposed() * v;
- }
+ math::Matrix<3, 3> Rvis = q.to_dcm();
+ math::Vector<3> v(1.0f, 0.0f, 0.4f);
- if (mocap_updated) {
- orb_copy(ORB_ID(att_pos_mocap), _mocap_sub, &_mocap);
- math::Quaternion q(_mocap.q);
- math::Matrix<3, 3> Rmoc = q.to_dcm();
-
- math::Vector<3> v(1.0f, 0.0f, 0.4f);
+ // Rvis is Rwr (robot respect to world) while v is respect to world.
+ // Hence Rvis must be transposed having (Rwr)' * Vw
+ // Rrw * Vw = vn. This way we have consistency
+ _vision_hdg = Rvis.transposed() * v;
- // Rmoc is Rwr (robot respect to world) while v is respect to world.
- // Hence Rmoc must be transposed having (Rwr)' * Vw
- // Rrw * Vw = vn. This way we have consistency
- _mocap_hdg = Rmoc.transposed() * v;
+ // vision external heading usage (ATT_EXT_HDG_M 1)
+ if (_ext_hdg_mode == 1) {
+ // Check for timeouts on data
+ _ext_hdg_good = vision.timestamp > 0 && (hrt_elapsed_time(&vision.timestamp) < 500000);
+ }
+ }
}
- // Update airspeed
- bool airspeed_updated = false;
- orb_check(_airspeed_sub, &airspeed_updated);
-
- if (airspeed_updated) {
- orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
- }
+ bool mocap_updated = false;
+ orb_check(_mocap_sub, &mocap_updated);
- // Check for timeouts on data
- if (_ext_hdg_mode == 1) {
- _ext_hdg_good = _vision.timestamp > 0 && (hrt_elapsed_time(&_vision.timestamp) < 500000);
+ if (mocap_updated) {
+ att_pos_mocap_s mocap;
- } else if (_ext_hdg_mode == 2) {
- _ext_hdg_good = _mocap.timestamp > 0 && (hrt_elapsed_time(&_mocap.timestamp) < 500000);
- }
+ if (orb_copy(ORB_ID(att_pos_mocap), _mocap_sub, &mocap) == PX4_OK) {
+ math::Quaternion q(mocap.q);
+ math::Matrix<3, 3> Rmoc = q.to_dcm();
- bool gpos_updated;
- orb_check(_global_pos_sub, &gpos_updated);
+ math::Vector<3> v(1.0f, 0.0f, 0.4f);
- if (gpos_updated) {
- orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_gpos);
+ // Rmoc is Rwr (robot respect to world) while v is respect to world.
+ // Hence Rmoc must be transposed having (Rwr)' * Vw
+ // Rrw * Vw = vn. This way we have consistency
+ _mocap_hdg = Rmoc.transposed() * v;
- if (_mag_decl_auto && _gpos.eph < 20.0f && hrt_elapsed_time(&_gpos.timestamp) < 1000000) {
- /* set magnetic declination automatically */
- update_mag_declination(math::radians(get_mag_declination(_gpos.lat, _gpos.lon)));
+ // Motion Capture external heading usage (ATT_EXT_HDG_M 2)
+ if (_ext_hdg_mode == 2) {
+ // Check for timeouts on data
+ _ext_hdg_good = mocap.timestamp > 0 && (hrt_elapsed_time(&mocap.timestamp) < 500000);
+ }
}
}
- if (_acc_comp && _gpos.timestamp != 0 && hrt_absolute_time() < _gpos.timestamp + 20000 && _gpos.eph < 5.0f && _inited) {
- /* position data is actual */
- if (gpos_updated) {
- Vector<3> vel(_gpos.vel_n, _gpos.vel_e, _gpos.vel_d);
+ bool gpos_updated = false;
+ orb_check(_global_pos_sub, &gpos_updated);
+
+ if (gpos_updated) {
+ vehicle_global_position_s gpos;
- /* 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);
+ if (orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &gpos) == PX4_OK) {
+ if (_mag_decl_auto && gpos.eph < 20.0f && hrt_elapsed_time(&gpos.timestamp) < 1000000) {
+ /* set magnetic declination automatically */
+ update_mag_declination(math::radians(get_mag_declination(gpos.lat, gpos.lon)));
}
- _vel_prev_t = _gpos.timestamp;
- _vel_prev = vel;
+ if (_acc_comp && gpos.timestamp != 0 && hrt_absolute_time() < gpos.timestamp + 20000 && gpos.eph < 5.0f && _inited) {
+ /* position data is actual */
+ 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) / 1e6f;
+ /* 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;
+ }
}
-
- } else {
- /* position data is outdated, reset acceleration */
- _pos_acc.zero();
- _vel_prev.zero();
- _vel_prev_t = 0;
}
/* time from previous iteration */
hrt_abstime now = hrt_absolute_time();
- float dt = (last_time > 0) ? ((now - last_time) / 1000000.0f) : 0.00001f;
+ const float dt = math::constrain((now - last_time) / 1e6f, _dt_min, _dt_max);
last_time = now;
- if (dt > _dt_max) {
- dt = _dt_max;
- }
-
- if (!update(dt)) {
- continue;
- }
-
- vehicle_attitude_s att = {
- .timestamp = sensors.timestamp,
- .rollspeed = _rates(0),
- .pitchspeed = _rates(1),
- .yawspeed = _rates(2),
-
- .q = {_q(0), _q(1), _q(2), _q(3)},
- .delta_q_reset = {},
- .quat_reset_counter = 0,
- };
+ if (update(dt)) {
+ vehicle_attitude_s att = {
+ .timestamp = sensors.timestamp,
+ .rollspeed = _rates(0),
+ .pitchspeed = _rates(1),
+ .yawspeed = _rates(2),
- /* the instance count is not used here */
- int att_inst;
- orb_publish_auto(ORB_ID(vehicle_attitude), &_att_pub, &att, &att_inst, ORB_PRIO_HIGH);
-
- {
- //struct estimator_status_s est = {};
-
- //est.timestamp = sensors.timestamp;
+ .q = {_q(0), _q(1), _q(2), _q(3)},
+ .delta_q_reset = {},
+ .quat_reset_counter = 0,
+ };
/* the instance count is not used here */
- //int est_inst;
- /* publish to control state topic */
- // TODO handle attitude states in position estimators instead so we can publish all data at once
- // or we need to enable more thatn just one estimator_status topic
- // orb_publish_auto(ORB_ID(estimator_status), &_est_state_pub, &est, &est_inst, ORB_PRIO_HIGH);
+ int att_inst;
+ orb_publish_auto(ORB_ID(vehicle_attitude), &_att_pub, &att, &att_inst, ORB_PRIO_HIGH);
}
}
@@ -485,12 +437,11 @@ void AttitudeEstimatorQ::task_main()
perf_end(_perf_mag);
#endif
+ orb_unsubscribe(_params_sub);
orb_unsubscribe(_sensors_sub);
+ orb_unsubscribe(_global_pos_sub);
orb_unsubscribe(_vision_sub);
orb_unsubscribe(_mocap_sub);
- orb_unsubscribe(_airspeed_sub);
- orb_unsubscribe(_params_sub);
- orb_unsubscribe(_global_pos_sub);
}
void AttitudeEstimatorQ::update_parameters(bool force)
@@ -509,18 +460,21 @@ void AttitudeEstimatorQ::update_parameters(bool force)
param_get(_params_handles.w_mag, &_w_mag);
param_get(_params_handles.w_ext_hdg, &_w_ext_hdg);
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);
update_mag_declination(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;
+ _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;
+ _acc_comp = (acc_comp_int != 0);
+
param_get(_params_handles.bias_max, &_bias_max);
param_get(_params_handles.ext_hdg_mode, &_ext_hdg_mode);
- param_get(_params_handles.airspeed_disabled, &_airspeed_disabled);
}
}
@@ -603,7 +557,7 @@ bool AttitudeEstimatorQ::update(float dt)
}
}
- if (_ext_hdg_mode == 0 || !_ext_hdg_good) {
+ if (_ext_hdg_mode == 0 || !_ext_hdg_good) {
// Magnetometer correction
// Project mag field vector to global frame and extract XY component
Vector<3> mag_earth = _q.conjugate(_mag);
@@ -726,7 +680,6 @@ int attitude_estimator_q_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (attitude_estimator_q::instance) {
- attitude_estimator_q::instance->print();
warnx("running");
return 0;