diff --git a/src/lib/terrain_estimation/terrain_estimator.cpp b/src/lib/terrain_estimation/terrain_estimator.cpp index 4f0cd4f110..7f33c6fc4e 100644 --- a/src/lib/terrain_estimation/terrain_estimator.cpp +++ b/src/lib/terrain_estimation/terrain_estimator.cpp @@ -66,8 +66,7 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu const struct sensor_combined_s *sensor, const struct distance_sensor_s *distance) { - matrix::Quaternion q(&attitude->q[0]); - matrix::Dcm R_att(q); + matrix::Dcmf R_att = matrix::Quatf(attitude->q); matrix::Vector a(&sensor->accelerometer_m_s2[0]); matrix::Vector u; u = R_att * a; @@ -112,8 +111,8 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl } if (distance->timestamp > _time_last_distance) { - matrix::Quaternion q(&attitude->q[0]); - matrix::Euler euler(q); + matrix::Quatf q(attitude->q); + matrix::Eulerf euler(q); float d = distance->current_distance; matrix::Matrix C; @@ -122,7 +121,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl float R = 0.009f; matrix::Vector y; - y(0) = d * cosf(euler(0)) * cosf(euler(1)); + y(0) = d * cosf(euler.phi()) * cosf(euler.theta()); // residual matrix::Matrix S_I = (C * _P * C.transpose()); diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index a6600fdc05..81e43dc76c 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -772,8 +772,8 @@ void Ekf2::task_main() lpos.ref_lon = ekf_origin.lon_rad * 180.0 / M_PI; // Reference point longitude in degrees // The rotation of the tangent plane vs. geographical north - matrix::Euler euler(q); - lpos.yaw = euler(2); + matrix::Eulerf euler(q); + lpos.yaw = euler.psi(); float terrain_vpos; lpos.dist_bottom_valid = _ekf.get_terrain_vert_pos(&terrain_vpos); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index afc47a16bc..fed9cd2db3 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -841,12 +841,11 @@ protected: if (_att_sub->update(&_att_time, &att)) { mavlink_attitude_t msg; - matrix::Quaternion q(&att.q[0]); - matrix::Euler euler(q); + matrix::Eulerf euler = matrix::Quatf(att.q); msg.time_boot_ms = att.timestamp / 1000; - msg.roll = euler(0); - msg.pitch = euler(1); - msg.yaw = euler(2); + msg.roll = euler.phi(); + msg.pitch = euler.theta(); + msg.yaw = euler.psi(); msg.rollspeed = att.rollspeed; msg.pitchspeed = att.pitchspeed; msg.yawspeed = att.yawspeed; @@ -1015,11 +1014,10 @@ protected: if (updated) { mavlink_vfr_hud_t msg; - matrix::Quaternion q(&att.q[0]); - matrix::Euler euler(q); + matrix::Eulerf euler = matrix::Quatf(att.q); msg.airspeed = airspeed.indicated_airspeed_m_s; msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); - msg.heading = _wrap_2pi(euler(2)) * M_RAD_TO_DEG_F; + msg.heading = _wrap_2pi(euler.psi()) * M_RAD_TO_DEG_F; msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; if (_pos_time > 0) { diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 80f06e11e0..a6b39b3cc0 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1942,8 +1942,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) { struct vehicle_global_position_s hil_global_pos; memset(&hil_global_pos, 0, sizeof(hil_global_pos)); - matrix::Quaternion q(&hil_attitude.q[0]); - matrix::Euler euler(q); + matrix::Eulerf euler = matrix::Quatf(hil_attitude.q); hil_global_pos.timestamp = timestamp; hil_global_pos.lat = hil_state.lat / ((double)1e7); hil_global_pos.lon = hil_state.lon / ((double)1e7); @@ -1951,7 +1950,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_global_pos.vel_n = hil_state.vx / 100.0f; hil_global_pos.vel_e = hil_state.vy / 100.0f; hil_global_pos.vel_d = hil_state.vz / 100.0f; - hil_global_pos.yaw = euler(2); + hil_global_pos.yaw = euler.psi(); hil_global_pos.eph = 2.0f; hil_global_pos.epv = 4.0f; @@ -1992,9 +1991,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_local_pos.vx = hil_state.vx / 100.0f; hil_local_pos.vy = hil_state.vy / 100.0f; hil_local_pos.vz = hil_state.vz / 100.0f; - matrix::Quaternion q(&hil_attitude.q[0]); - matrix::Euler euler(q); - hil_local_pos.yaw = euler(2); + matrix::Eulerf euler = matrix::Quatf(hil_attitude.q); + hil_local_pos.yaw = euler.psi(); hil_local_pos.xy_global = true; hil_local_pos.z_global = true; 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 1ad8ad2709..cbd527ab3f 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -1330,8 +1330,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.vy = y_est[1]; local_pos.z = z_est[0]; local_pos.vz = z_est[1]; - matrix::Euler euler(R); - local_pos.yaw = euler(2); + matrix::Eulerf euler(R); + local_pos.yaw = euler.psi(); local_pos.dist_bottom_valid = dist_bottom_valid; local_pos.eph = eph; local_pos.epv = epv; diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 8d0af17f83..d8e875a953 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -131,9 +131,8 @@ void Tailsitter::update_vtol_state() * For the backtransition the pitch is controlled in MC mode again and switches to full MC control reaching the sufficient pitch angle. */ - matrix::Quaternion q(&_v_att->q[0]); - matrix::Euler euler(q); - float pitch = euler(1); + matrix::Eulerf euler = matrix::Quatf(_v_att->q); + float pitch = euler.theta(); if (!_attc->is_fixed_wing_requested()) {