diff --git a/src/drivers/bst/bst.cpp b/src/drivers/bst/bst.cpp index d131ab2723..b761bea4e8 100644 --- a/src/drivers/bst/bst.cpp +++ b/src/drivers/bst/bst.cpp @@ -51,6 +51,8 @@ #include #include +using namespace matrix; + #define BST_DEVICE_PATH "/dev/bst0" static const char commandline_usage[] = "usage: bst start|status|stop"; @@ -290,13 +292,13 @@ void BST::cycle() if (updated) { vehicle_attitude_s att; orb_copy(ORB_ID(vehicle_attitude), _attitude_sub, &att); - matrix::Quaternion q(&att.q[0]); - matrix::Euler euler(q); + Quatf q(att.q); + Eulerf euler(q); BSTPacket bst_att = {}; bst_att.type = 0x1E; - bst_att.payload.roll = swap_int32(euler(0) * 10000); - bst_att.payload.pitch = swap_int32(euler(1) * 10000); - bst_att.payload.yaw = swap_int32(euler(2) * 10000); + bst_att.payload.roll = swap_int32(euler.phi() * 10000); + bst_att.payload.pitch = swap_int32(euler.theta() * 10000); + bst_att.payload.yaw = swap_int32(euler.psi() * 10000); send_packet(bst_att); } diff --git a/src/drivers/vmount/output.cpp b/src/drivers/vmount/output.cpp index f804c631f9..6ed8e2a6ec 100644 --- a/src/drivers/vmount/output.cpp +++ b/src/drivers/vmount/output.cpp @@ -187,8 +187,7 @@ void OutputBase::_calculate_output_angles(const hrt_abstime &t) orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &vehicle_attitude); } - matrix::Quaternion q(&vehicle_attitude.q[0]); - matrix::Euler euler(q); + matrix::Eulerf euler = matrix::Quatf(vehicle_attitude.q); for (int i = 0; i < 3; ++i) { if (_stabilize[i]) { 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 edb12cc9ca..361d8a2da7 100755 --- a/src/examples/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/examples/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -611,10 +611,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) att.yawspeed = x_aposteriori[2]; /* magnetic declination */ - matrix::Dcm Ro(&Rot_matrix[0]); - matrix::Dcm R_declination(&R_decl.data[0][0]); + matrix::Dcmf Ro(&Rot_matrix[0]); + matrix::Dcmf R_declination(&R_decl.data[0][0]); Ro = R_declination * Ro; - matrix::Quaternion q(Ro); + matrix::Quatf q = R_declination * Ro; memcpy(&att.q[0],&q._data[0],sizeof(att.q)); 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 d204a161b4..7cff56de42 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 @@ -952,9 +952,8 @@ void AttitudePositionEstimatorEKF::publishLocalPosition() _local_pos.xy_global = _gps_initialized; //TODO: Handle optical flow mode here _local_pos.z_global = false; - matrix::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]); - matrix::Euler euler(q); - _local_pos.yaw = euler(2); + matrix::Eulerf euler = matrix::Quatf(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]); + _local_pos.yaw = euler.psi(); if (!PX4_ISFINITE(_local_pos.x) || !PX4_ISFINITE(_local_pos.y) || diff --git a/src/examples/fixedwing_control/main.cpp b/src/examples/fixedwing_control/main.cpp index 9b2a25de19..132726552b 100644 --- a/src/examples/fixedwing_control/main.cpp +++ b/src/examples/fixedwing_control/main.cpp @@ -177,19 +177,16 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st * Calculate roll error and apply P gain */ - matrix::Quaternion qa(&att->q[0]); - matrix::Euler att_euler(qa); + matrix::Eulerf att_euler = matrix::Quatf(att->q); + matrix::Eulerf att_sp_euler = matrix::Quatf(att_sp->q_d); - matrix::Quaternion qd(&att_sp->q_d[0]); - matrix::Euler att_sp_euler(qd); - - float roll_err = att_euler(0) - att_sp_euler(0); + float roll_err = att_euler.phi() - att_sp_euler.phi(); actuators->control[0] = roll_err * p.roll_p; /* * Calculate pitch error and apply P gain */ - float pitch_err = att_euler(1) - att_sp_euler(1); + float pitch_err = att_euler.theta() - att_sp_euler.theta(); actuators->control[1] = pitch_err * p.pitch_p; } @@ -203,11 +200,10 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct p float bearing = get_bearing_to_next_waypoint(pos->lat, pos->lon, sp->lat, sp->lon); - matrix::Quaternion qa(&att->q[0]); - matrix::Euler att_euler(qa); + matrix::Eulerf att_euler = matrix::Quatf(att->q); /* calculate heading error */ - float yaw_err = att_euler(2) - bearing; + float yaw_err = att_euler.psi() - bearing; /* apply control gain */ float roll_body = yaw_err * p.hdng_p; @@ -219,9 +215,9 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct p roll_body = 0.6f; } - matrix::Euler att_spe(roll_body, 0, bearing); + matrix::Eulerf att_spe(roll_body, 0, bearing); - matrix::Quaternion qd(att_spe); + matrix::Quatf qd(att_spe); att_sp->q_d[0] = qd(0); att_sp->q_d[1] = qd(1); diff --git a/src/examples/rover_steering_control/main.cpp b/src/examples/rover_steering_control/main.cpp index abf3c94d74..388fe55d62 100644 --- a/src/examples/rover_steering_control/main.cpp +++ b/src/examples/rover_steering_control/main.cpp @@ -179,11 +179,7 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st /* * Calculate roll error and apply P gain */ - matrix::Quaternion q(&att->q[0]); - matrix::Euler euler(q); - matrix::Quaternion q_sp(&att_sp->q_d[0]); - matrix::Euler euler_sp(q_sp); - float yaw_err = euler(2) - euler_sp(2); + float yaw_err = Eulerf(Quaternion(att->q)).psi() - Eulerf(Quaternion(att->q_d)).psi(); actuators->control[2] = yaw_err * pp.yaw_p; /* copy throttle */ diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 1cf0d027d4..ae9925eff6 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -687,10 +687,9 @@ int do_level_calibration(orb_advert_t *mavlink_log_pub) { } orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - matrix::Quaternion q(&att.q[0]); - matrix::Euler euler(q); - roll_mean += euler(0); - pitch_mean += euler(1); + matrix::Eulerf euler = matrix::Quatf(att.q); + roll_mean += euler.phi(); + pitch_mean += euler.theta(); counter++; } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5856405b5b..b7be1a8ccc 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1168,9 +1168,8 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s & home.y = localPosition.y; home.z = localPosition.z; - matrix::Quaternion q(&attitude.q[0]); - matrix::Euler euler(q); - home.yaw = euler(2); + matrix::Eulerf euler = matrix::Quatf(attitude.q); + home.yaw = euler.psi(); PX4_INFO("home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); diff --git a/src/modules/local_position_estimator/sensors/flow.cpp b/src/modules/local_position_estimator/sensors/flow.cpp index 1a6ca8c2c1..2bfe6dc677 100644 --- a/src/modules/local_position_estimator/sensors/flow.cpp +++ b/src/modules/local_position_estimator/sensors/flow.cpp @@ -64,10 +64,9 @@ int BlockLocalPositionEstimator::flowMeasure(Vector &y) return -1; } - matrix::Quaternion q(&_sub_att.get().q[0]); - matrix::Euler euler(q); + matrix::Eulerf euler = matrix::Quatf(_sub_att.get().q); - float d = agl() * cosf(euler(0)) * cosf(euler(1)); + float d = agl() * cosf(euler.phi()) * cosf(euler.theta()); // optical flow in x, y axis // TODO consider making flow scale a states of the kalman filter 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 d3e785db52..1ad8ad2709 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -500,8 +500,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* sensor combined */ orb_check(sensor_combined_sub, &updated); - matrix::Quaternion q(&att.q[0]); - matrix::Dcm R(q); + matrix::Dcmf R = matrix::Quatf(att.q); if (updated) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); @@ -944,8 +943,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - matrix::Quaternion q(&att.q[0]); - matrix::Dcm R(q); + matrix::Dcm R = matrix::Quatf(att.q); /* check for timeout on FLOW topic */ if ((flow_valid || lidar_valid) && t > (flow_time + flow_topic_timeout)) {