forked from Archive/PX4-Autopilot
attitude_estimator_q remove unused (#8106)
This commit is contained in:
Daniel Agar
GitHub
parent
4da4093839
commit
c47cd972a8
|
|
@ -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 <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/err.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/mavlink_log.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_global_position.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;
|
||||
hrt_abstime _vel_prev_t = 0;
|
||||
|
||||
Vector<3> _pos_acc;
|
||||
|
||||
/* 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;
|
||||
|
||||
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);
|
||||
|
||||
if (vision_updated) {
|
||||
vehicle_attitude_s vision;
|
||||
|
||||
if (orb_copy(ORB_ID(vehicle_vision_attitude), _vision_sub, &vision) == PX4_OK) {
|
||||
math::Quaternion q(vision.q);
|
||||
|
||||
math::Matrix<3, 3> Rvis = 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;
|
||||
|
||||
// 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
math::Matrix<3, 3> Rvis = 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;
|
||||
}
|
||||
|
||||
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();
|
||||
att_pos_mocap_s mocap;
|
||||
|
||||
math::Vector<3> v(1.0f, 0.0f, 0.4f);
|
||||
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();
|
||||
|
||||
// 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;
|
||||
math::Vector<3> v(1.0f, 0.0f, 0.4f);
|
||||
|
||||
// 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;
|
||||
|
||||
// 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Update airspeed
|
||||
bool airspeed_updated = false;
|
||||
orb_check(_airspeed_sub, &airspeed_updated);
|
||||
|
||||
if (airspeed_updated) {
|
||||
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
|
||||
}
|
||||
|
||||
// Check for timeouts on data
|
||||
if (_ext_hdg_mode == 1) {
|
||||
_ext_hdg_good = _vision.timestamp > 0 && (hrt_elapsed_time(&_vision.timestamp) < 500000);
|
||||
|
||||
} else if (_ext_hdg_mode == 2) {
|
||||
_ext_hdg_good = _mocap.timestamp > 0 && (hrt_elapsed_time(&_mocap.timestamp) < 500000);
|
||||
}
|
||||
|
||||
bool gpos_updated;
|
||||
bool gpos_updated = false;
|
||||
orb_check(_global_pos_sub, &gpos_updated);
|
||||
|
||||
if (gpos_updated) {
|
||||
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_gpos);
|
||||
vehicle_global_position_s gpos;
|
||||
|
||||
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)));
|
||||
}
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
/* 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);
|
||||
|
||||
} else {
|
||||
/* position data is outdated, reset acceleration */
|
||||
_pos_acc.zero();
|
||||
_vel_prev.zero();
|
||||
_vel_prev_t = 0;
|
||||
/* 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* 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)) {
|
||||
vehicle_attitude_s att = {
|
||||
.timestamp = sensors.timestamp,
|
||||
.rollspeed = _rates(0),
|
||||
.pitchspeed = _rates(1),
|
||||
.yawspeed = _rates(2),
|
||||
|
||||
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,
|
||||
};
|
||||
|
||||
/* 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;
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue