void AttitudeEstimatorQ::print()
{
warnx("gyro status:");
_voter_gyro.print();
warnx("accel status:");
_voter_accel.print();
warnx("mag status:");
_voter_mag.print();
}
void AttitudeEstimatorQ::task_main()
{
_sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
_vision_sub = orb_subscribe(ORB_ID(vision_position_estimate));
_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
_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;
px4_pollfd_struct_t fds[1];
fds[0].fd = _sensors_sub;
fds[0].events = POLLIN;
while (!_task_should_exit) {
int ret = px4_poll(fds, 1, 1000);
if (_mavlink_fd < 0) {
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
}
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)) {
// Feed validator with recent sensor data
for (unsigned i = 0; i < (sizeof(sensors.gyro_timestamp) / sizeof(sensors.gyro_timestamp[0])); i++) {
/* ignore empty fields */
if (sensors.gyro_timestamp[i] > 0) {
float gyro[3];
for (unsigned j = 0; j < 3; j++) {
if (sensors.gyro_integral_dt[i] > 0) {
gyro[j] = (double)sensors.gyro_integral_rad[i * 3 + j] / (sensors.gyro_integral_dt[i] / 1e6);
} else {
/* fall back to angular rate */
gyro[j] = sensors.gyro_rad_s[i * 3 + j];
}
}
_voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]);
}
_voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3],
sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]);
_voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3],
sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]);
}
int best_gyro, best_accel, best_mag;
// Get best measurement values
hrt_abstime curr_time = hrt_absolute_time();
_gyro.set(_voter_gyro.get_best(curr_time, &best_gyro));
_accel.set(_voter_accel.get_best(curr_time, &best_accel));
_mag.set(_voter_mag.get_best(curr_time, &best_mag));
if (_accel.length() < 0.01f || _mag.length() < 0.01f) {
warnx("WARNING: degenerate accel / mag!");
continue;
}
_data_good = true;
if (!_failsafe && (_voter_gyro.failover_count() > 0 ||
_voter_accel.failover_count() > 0 ||
_voter_mag.failover_count() > 0)) {
_failsafe = true;
mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
}
if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
_voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
_voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) {
if (_vibration_warning_timestamp == 0) {
_vibration_warning_timestamp = curr_time;
} else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000) {
_vibration_warning = true;
mavlink_and_console_log_critical(_mavlink_fd, "HIGH VIBRATION! g: %d a: %d m: %d",
(int)(100 * _voter_gyro.get_vibration_factor(curr_time)),
(int)(100 * _voter_accel.get_vibration_factor(curr_time)),
(int)(100 * _voter_mag.get_vibration_factor(curr_time)));
}
} else {
_vibration_warning_timestamp = 0;
}
}
// Update vision and motion capture heading
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(vision_position_estimate), _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();
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;
}
// Check for timeouts on data
if (_ext_hdg_mode == 1) {
_ext_hdg_good = _vision.timestamp_boot > 0 && (hrt_elapsed_time(&_vision.timestamp_boot) < 500000);
} else if (_ext_hdg_mode == 2) {
_ext_hdg_good = _mocap.timestamp_boot > 0 && (hrt_elapsed_time(&_mocap.timestamp_boot) < 500000);
}
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 && _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);
}
_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;
last_time = now;
if (dt > _dt_max) {
dt = _dt_max;
}
if (!update(dt)) {
continue;
}
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);
/* the complimentary filter should reflect the true system
* state, but we need smoother outputs for the control system
*/
att.rollspeed = _lp_roll_rate.apply(_rates(0));
att.pitchspeed = _lp_pitch_rate.apply(_rates(1));
att.yawspeed = _lp_yaw_rate.apply(_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;
att.rate_vibration = _voter_gyro.get_vibration_factor(hrt_absolute_time());
att.accel_vibration = _voter_accel.get_vibration_factor(hrt_absolute_time());
att.mag_vibration = _voter_mag.get_vibration_factor(hrt_absolute_time());
if (_att_pub == nullptr) {
_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
} else {
orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
}
struct control_state_s ctrl_state = {};
ctrl_state.timestamp = sensors.timestamp;
/* Attitude quaternions for control state */
ctrl_state.q[0] = _q(0);
ctrl_state.q[1] = _q(1);
ctrl_state.q[2] = _q(2);
ctrl_state.q[3] = _q(3);
/* Attitude rates for control state */
ctrl_state.roll_rate = _lp_roll_rate.apply(_rates(0));
ctrl_state.pitch_rate = _lp_pitch_rate.apply(_rates(1));
ctrl_state.yaw_rate = _rates(2);
/* Publish to control state topic */
if (_ctrl_state_pub == nullptr) {
_ctrl_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);
} else {
orb_publish(ORB_ID(control_state), _ctrl_state_pub, &ctrl_state);
}
}
}
void AttitudeEstimatorQ::task_main()
{
#ifdef __PX4_POSIX
perf_counter_t _perf_accel(perf_alloc_once(PC_ELAPSED, "sim_accel_delay"));
perf_counter_t _perf_mpu(perf_alloc_once(PC_ELAPSED, "sim_mpu_delay"));
perf_counter_t _perf_mag(perf_alloc_once(PC_ELAPSED, "sim_mag_delay"));
#endif
_sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
_vision_sub = orb_subscribe(ORB_ID(vision_position_estimate));
_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));
update_parameters(true);
hrt_abstime last_time = 0;
px4_pollfd_struct_t fds[1] = {};
fds[0].fd = _sensors_sub;
fds[0].events = POLLIN;
while (!_task_should_exit) {
int ret = px4_poll(fds, 1, 1000);
if (ret < 0) {
// Poll error, sleep and try again
usleep(10000);
PX4_WARN("Q POLL ERROR");
continue;
} else if (ret == 0) {
// Poll timeout, do nothing
PX4_WARN("Q POLL TIMEOUT");
continue;
}
update_parameters(false);
// Update sensors
sensor_combined_s sensors;
int best_gyro = 0;
int best_accel = 0;
int best_mag = 0;
if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) {
// Feed validator with recent sensor data
for (unsigned i = 0; i < (sizeof(sensors.gyro_timestamp) / sizeof(sensors.gyro_timestamp[0])); i++) {
/* ignore empty fields */
if (sensors.gyro_timestamp[i] > 0) {
float gyro[3];
for (unsigned j = 0; j < 3; j++) {
if (sensors.gyro_integral_dt[i] > 0) {
gyro[j] = (double)sensors.gyro_integral_rad[i * 3 + j] / (sensors.gyro_integral_dt[i] / 1e6);
} else {
/* fall back to angular rate */
gyro[j] = sensors.gyro_rad_s[i * 3 + j];
}
}
_voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]);
}
/* ignore empty fields */
if (sensors.accelerometer_timestamp[i] > 0) {
_voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3],
sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]);
}
/* ignore empty fields */
if (sensors.magnetometer_timestamp[i] > 0) {
_voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3],
sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]);
}
}
// Get best measurement values
hrt_abstime curr_time = hrt_absolute_time();
_gyro.set(_voter_gyro.get_best(curr_time, &best_gyro));
_accel.set(_voter_accel.get_best(curr_time, &best_accel));
_mag.set(_voter_mag.get_best(curr_time, &best_mag));
if (_accel.length() < 0.01f) {
warnx("WARNING: degenerate accel!");
continue;
}
if (_mag.length() < 0.01f) {
warnx("WARNING: degenerate mag!");
continue;
}
_data_good = true;
if (!_failsafe) {
uint32_t flags = DataValidator::ERROR_FLAG_NO_ERROR;
#ifdef __PX4_POSIX
perf_end(_perf_accel);
perf_end(_perf_mpu);
perf_end(_perf_mag);
#endif
if (_voter_gyro.failover_count() > 0) {
_failsafe = true;
flags = _voter_gyro.failover_state();
mavlink_and_console_log_emergency(&_mavlink_log_pub, "Gyro #%i failure :%s%s%s%s%s!",
_voter_gyro.failover_index(),
((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
}
if (_voter_accel.failover_count() > 0) {
_failsafe = true;
flags = _voter_accel.failover_state();
mavlink_and_console_log_emergency(&_mavlink_log_pub, "Accel #%i failure :%s%s%s%s%s!",
_voter_accel.failover_index(),
((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
}
if (_voter_mag.failover_count() > 0) {
_failsafe = true;
flags = _voter_mag.failover_state();
mavlink_and_console_log_emergency(&_mavlink_log_pub, "Mag #%i failure :%s%s%s%s%s!",
_voter_mag.failover_index(),
((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : ""));
}
if (_failsafe) {
mavlink_and_console_log_emergency(&_mavlink_log_pub, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
}
}
if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
_voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
_voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) {
if (_vibration_warning_timestamp == 0) {
_vibration_warning_timestamp = curr_time;
} else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000) {
_vibration_warning = true;
mavlink_and_console_log_critical(&_mavlink_log_pub, "HIGH VIBRATION! g: %d a: %d m: %d",
(int)(100 * _voter_gyro.get_vibration_factor(curr_time)),
(int)(100 * _voter_accel.get_vibration_factor(curr_time)),
(int)(100 * _voter_mag.get_vibration_factor(curr_time)));
}
} else {
_vibration_warning_timestamp = 0;
}
}
// Update vision and motion capture heading
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(vision_position_estimate), _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();
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;
}
// 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_boot > 0 && (hrt_elapsed_time(&_vision.timestamp_boot) < 500000);
} else if (_ext_hdg_mode == 2) {
_ext_hdg_good = _mocap.timestamp_boot > 0 && (hrt_elapsed_time(&_mocap.timestamp_boot) < 500000);
}
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 */
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);
}
_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;
last_time = now;
if (dt > _dt_max) {
dt = _dt_max;
}
if (!update(dt)) {
continue;
}
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;
memcpy(&att.q[0], _q.data, sizeof(att.q));
att.q_valid = true;
att.rate_vibration = _voter_gyro.get_vibration_factor(hrt_absolute_time());
att.accel_vibration = _voter_accel.get_vibration_factor(hrt_absolute_time());
att.mag_vibration = _voter_mag.get_vibration_factor(hrt_absolute_time());
/* 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 control_state_s ctrl_state = {};
ctrl_state.timestamp = sensors.timestamp;
/* attitude quaternions for control state */
ctrl_state.q[0] = _q(0);
ctrl_state.q[1] = _q(1);
ctrl_state.q[2] = _q(2);
ctrl_state.q[3] = _q(3);
/* attitude rates for control state */
ctrl_state.roll_rate = _lp_roll_rate.apply(_rates(0));
ctrl_state.pitch_rate = _lp_pitch_rate.apply(_rates(1));
ctrl_state.yaw_rate = _lp_yaw_rate.apply(_rates(2));
/* Airspeed - take airspeed measurement directly here as no wind is estimated */
if (PX4_ISFINITE(_airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - _airspeed.timestamp < 1e6
&& _airspeed.timestamp > 0) {
ctrl_state.airspeed = _airspeed.indicated_airspeed_m_s;
ctrl_state.airspeed_valid = true;
} else {
ctrl_state.airspeed_valid = false;
}
/* the instance count is not used here */
int ctrl_inst;
/* publish to control state topic */
orb_publish_auto(ORB_ID(control_state), &_ctrl_state_pub, &ctrl_state, &ctrl_inst, ORB_PRIO_HIGH);
}
{
struct estimator_status_s est = {};
est.timestamp = sensors.timestamp;
est.vibe[0] = _voter_accel.get_vibration_offset(est.timestamp, 0);
est.vibe[1] = _voter_accel.get_vibration_offset(est.timestamp, 1);
est.vibe[2] = _voter_accel.get_vibration_offset(est.timestamp, 2);
/* the instance count is not used here */
int est_inst;
/* publish to control state topic */
orb_publish_auto(ORB_ID(estimator_status), &_est_state_pub, &est, &est_inst, ORB_PRIO_HIGH);
}
}
}
