From 526fb8f5151ec73e7dc9f1d39a587f3146825e09 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Sep 2016 17:54:04 +0200 Subject: [PATCH] Remove q_valid flag from attitude topic --- msg/vehicle_attitude.msg | 3 +- .../attitude_estimator_ekf_main.cpp | 3 +- .../ekf_att_pos_estimator_main.cpp | 1 - .../terrain_estimation/terrain_estimator.cpp | 18 ++++------ .../attitude_estimator_q_main.cpp | 1 - src/modules/ekf2/ekf2_main.cpp | 1 - src/modules/ekf2_replay/ekf2_replay_main.cpp | 12 +++---- .../BlockLocalPositionEstimator.cpp | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 1 - .../position_estimator_inav_main.cpp | 34 ++++++++----------- src/modules/simulator/simulator_mavlink.cpp | 1 - 11 files changed, 31 insertions(+), 46 deletions(-) diff --git a/msg/vehicle_attitude.msg b/msg/vehicle_attitude.msg index 8e0f594a8e..6761765640 100644 --- a/msg/vehicle_attitude.msg +++ b/msg/vehicle_attitude.msg @@ -1,8 +1,7 @@ -# This is similar to the mavlink message ATTITUDE, but for onboard use +# This is similar to the mavlink message ATTITUDE_QUATERNION, but for onboard use float32 rollspeed # Angular velocity about body north axis (x) in rad/s float32 pitchspeed # Angular velocity about body east axis (y) in rad/s float32 yawspeed # Angular velocity about body down axis (z) in rad/s float32[4] q # Quaternion (NED) -bool q_valid # Quaternion valid # TOPICS vehicle_attitude vehicle_attitude_groundtruth diff --git a/src/examples/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/examples/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index f27e7697b1..edb12cc9ca 100755 --- a/src/examples/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/examples/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -617,7 +617,6 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) matrix::Quaternion q(Ro); memcpy(&att.q[0],&q._data[0],sizeof(att.q)); - att.q_valid = true; if (PX4_ISFINITE(att.q[0]) && PX4_ISFINITE(att.q[1]) && PX4_ISFINITE(att.q[2]) && PX4_ISFINITE(att.q[3])) { @@ -625,7 +624,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); } else { - warnx("ERR: NaN estimate!"); + PX4_ERR("ERR: NaN estimate!"); } perf_end(ekf_loop_perf); diff --git a/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 8e5ec35817..d204a161b4 100644 --- a/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -826,7 +826,6 @@ void AttitudePositionEstimatorEKF::publishAttitude() _att.q[1] = _ekf->states[1]; _att.q[2] = _ekf->states[2]; _att.q[3] = _ekf->states[3]; - _att.q_valid = true; _att.rollspeed = _ekf->dAngIMU.x / _ekf->dtIMU - _ekf->states[10] / _ekf->dtIMUfilt; _att.pitchspeed = _ekf->dAngIMU.y / _ekf->dtIMU - _ekf->states[11] / _ekf->dtIMUfilt; diff --git a/src/lib/terrain_estimation/terrain_estimator.cpp b/src/lib/terrain_estimation/terrain_estimator.cpp index 9d83c07efc..4f0cd4f110 100644 --- a/src/lib/terrain_estimation/terrain_estimator.cpp +++ b/src/lib/terrain_estimation/terrain_estimator.cpp @@ -37,6 +37,7 @@ */ #include "terrain_estimator.h" +#include #define DISTANCE_TIMEOUT 100000 // time in usec after which laser is considered dead @@ -65,17 +66,12 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu const struct sensor_combined_s *sensor, const struct distance_sensor_s *distance) { - if (attitude->q_valid) { - matrix::Quaternion q(&attitude->q[0]); - matrix::Dcm R_att(q); - matrix::Vector a(&sensor->accelerometer_m_s2[0]); - matrix::Vector u; - u = R_att * a; - _u_z = u(2) + 9.81f; // compensate for gravity - - } else { - _u_z = 0.0f; - } + matrix::Quaternion q(&attitude->q[0]); + matrix::Dcm R_att(q); + matrix::Vector a(&sensor->accelerometer_m_s2[0]); + matrix::Vector u; + u = R_att * a; + _u_z = u(2) + CONSTANTS_ONE_G; // compensate for gravity // dynamics matrix matrix::Matrix A; 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 8fb97526a5..63c068aab6 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -473,7 +473,6 @@ void AttitudeEstimatorQ::task_main() att.yawspeed = _rates(2); memcpy(&att.q[0], _q.data, sizeof(att.q)); - att.q_valid = true; /* the instance count is not used here */ int att_inst; diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 149ce4d30d..a6600fdc05 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -726,7 +726,6 @@ void Ekf2::task_main() att.q[1] = q(1); att.q[2] = q(2); att.q[3] = q(3); - att.q_valid = true; att.rollspeed = gyro_rad[0]; att.pitchspeed = gyro_rad[1]; diff --git a/src/modules/ekf2_replay/ekf2_replay_main.cpp b/src/modules/ekf2_replay/ekf2_replay_main.cpp index 3338a4be66..65865c1616 100644 --- a/src/modules/ekf2_replay/ekf2_replay_main.cpp +++ b/src/modules/ekf2_replay/ekf2_replay_main.cpp @@ -535,15 +535,15 @@ void Ekf2Replay::logIfUpdated() log_message.body.att.q_x = att.q[1]; log_message.body.att.q_y = att.q[2]; log_message.body.att.q_z = att.q[3]; - log_message.body.att.roll = att.roll; - log_message.body.att.pitch = att.pitch; - log_message.body.att.yaw = att.yaw; + log_message.body.att.roll = 0; + log_message.body.att.pitch = 0; + log_message.body.att.yaw = 0; log_message.body.att.roll_rate = att.rollspeed; log_message.body.att.pitch_rate = att.pitchspeed; log_message.body.att.yaw_rate = att.yawspeed; - log_message.body.att.gx = att.g_comp[0]; - log_message.body.att.gy = att.g_comp[1]; - log_message.body.att.gz = att.g_comp[2]; + log_message.body.att.gx = 0; + log_message.body.att.gy = 0; + log_message.body.att.gz = 0; writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_ATT_MSG].length); diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 093fe85cea..0101ce9802 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -929,7 +929,7 @@ void BlockLocalPositionEstimator::predict() // state or covariance if (!_validXY && !_validZ) { return; } - if (integrate && _sub_att.get().q_valid) { + if (integrate) { matrix::Quaternion q(&_sub_att.get().q[0]); _eul = matrix::Euler(q); _R_att = matrix::Dcm(q); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 1a08ae1449..80f06e11e0 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1925,7 +1925,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_attitude.q[1] = q(1); hil_attitude.q[2] = q(2); hil_attitude.q[3] = q(3); - hil_attitude.q_valid = true; hil_attitude.rollspeed = hil_state.rollspeed; hil_attitude.pitchspeed = hil_state.pitchspeed; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp index 0ccb97d5ff..d3e785db52 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -507,27 +507,22 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); if (sensor.timestamp + sensor.accelerometer_timestamp_relative != accel_timestamp) { - if (att.q_valid) { - /* correct accel bias */ - sensor.accelerometer_m_s2[0] -= acc_bias[0]; - sensor.accelerometer_m_s2[1] -= acc_bias[1]; - sensor.accelerometer_m_s2[2] -= acc_bias[2]; + /* correct accel bias */ + sensor.accelerometer_m_s2[0] -= acc_bias[0]; + sensor.accelerometer_m_s2[1] -= acc_bias[1]; + sensor.accelerometer_m_s2[2] -= acc_bias[2]; - /* transform acceleration vector from body frame to NED frame */ - for (int i = 0; i < 3; i++) { - acc[i] = 0.0f; + /* transform acceleration vector from body frame to NED frame */ + for (int i = 0; i < 3; i++) { + acc[i] = 0.0f; - for (int j = 0; j < 3; j++) { - acc[i] += R(i, j) * sensor.accelerometer_m_s2[j]; - } + for (int j = 0; j < 3; j++) { + acc[i] += R(i, j) * sensor.accelerometer_m_s2[j]; } - - acc[2] += CONSTANTS_ONE_G; - - } else { - memset(acc, 0, sizeof(acc)); } + acc[2] += CONSTANTS_ONE_G; + accel_timestamp = sensor.timestamp + sensor.accelerometer_timestamp_relative; accel_updates++; } @@ -551,8 +546,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { //check if altitude estimation for lidar is enabled and new sensor data - if (params.enable_lidar_alt_est && lidar.current_distance > lidar.min_distance && lidar.current_distance < lidar.max_distance - && (R(2, 2) > 0.7f)) { + if (params.enable_lidar_alt_est && lidar.current_distance > lidar.min_distance + && lidar.current_distance < lidar.max_distance + && (R(2, 2) > 0.7f)) { if (!use_lidar_prev && use_lidar) { lidar_first = true; @@ -614,7 +610,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float body_v_est[2] = { 0.0f, 0.0f }; for (int i = 0; i < 2; i++) { - body_v_est[i] = R( 0, i) * x_est[1] + R(1, i) * y_est[1] + R(2, i) * z_est[1]; + body_v_est[i] = R(0, i) * x_est[1] + R(1, i) * y_est[1] + R(2, i) * z_est[1]; } /* set this flag if flow should be accurate according to current velocity and attitude rate estimate */ diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 371e071ecc..53fd732467 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -321,7 +321,6 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish) hil_attitude.q[1] = q(1); hil_attitude.q[2] = q(2); hil_attitude.q[3] = q(3); - hil_attitude.q_valid = true; hil_attitude.rollspeed = hil_state.rollspeed; hil_attitude.pitchspeed = hil_state.pitchspeed;