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); } } }