fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 04:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Quaternion: rename function to rotate vectors

Browse Source
This commit is contained in:
Matthias Grob
2022-03-07 20:03:54 -05:00
committed by Daniel Agar
parent 5affa693f2
commit 68a0414622
7 changed files with 27 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+8 -8
View File
@@ -327,7 +327,7 @@ AttitudeEstimatorQ::Run()
if (_vel_prev_t != 0 && lpos.timestamp != _vel_prev_t) {
float vel_dt = (lpos.timestamp - _vel_prev_t) / 1e6f;
/* calculate acceleration in body frame */
_pos_acc = _q.conjugate_inversed((vel - _vel_prev) / vel_dt);
_pos_acc = _q.rotateVectorInverse((vel - _vel_prev) / vel_dt);
}
_vel_prev_t = lpos.timestamp;
@@ -452,26 +452,26 @@ AttitudeEstimatorQ::update(float dt)
if (_param_att_ext_hdg_m.get() == 1) {
// Vision heading correction
// Project heading to global frame and extract XY component
Vector3f vision_hdg_earth = _q.conjugate(_vision_hdg);
Vector3f vision_hdg_earth = _q.rotateVector(_vision_hdg);
float vision_hdg_err = wrap_pi(atan2f(vision_hdg_earth(1), vision_hdg_earth(0)));
// Project correction to body frame
corr += _q.conjugate_inversed(Vector3f(0.0f, 0.0f, -vision_hdg_err)) * _param_att_w_ext_hdg.get();
corr += _q.rotateVectorInverse(Vector3f(0.0f, 0.0f, -vision_hdg_err)) * _param_att_w_ext_hdg.get();
}
if (_param_att_ext_hdg_m.get() == 2) {
// Mocap heading correction
// Project heading to global frame and extract XY component
Vector3f mocap_hdg_earth = _q.conjugate(_mocap_hdg);
Vector3f mocap_hdg_earth = _q.rotateVector(_mocap_hdg);
float mocap_hdg_err = wrap_pi(atan2f(mocap_hdg_earth(1), mocap_hdg_earth(0)));
// Project correction to body frame
corr += _q.conjugate_inversed(Vector3f(0.0f, 0.0f, -mocap_hdg_err)) * _param_att_w_ext_hdg.get();
corr += _q.rotateVectorInverse(Vector3f(0.0f, 0.0f, -mocap_hdg_err)) * _param_att_w_ext_hdg.get();
}
}
if (_param_att_ext_hdg_m.get() == 0 || !_ext_hdg_good) {
// Magnetometer correction
// Project mag field vector to global frame and extract XY component
Vector3f mag_earth = _q.conjugate(_mag);
Vector3f mag_earth = _q.rotateVector(_mag);
float mag_err = wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
float gainMult = 1.0f;
const float fifty_dps = 0.873f;
@@ -481,7 +481,7 @@ AttitudeEstimatorQ::update(float dt)
}
// Project magnetometer correction to body frame
corr += _q.conjugate_inversed(Vector3f(0.0f, 0.0f, -mag_err)) * _param_att_w_mag.get() * gainMult;
corr += _q.rotateVectorInverse(Vector3f(0.0f, 0.0f, -mag_err)) * _param_att_w_mag.get() * gainMult;
}
_q.normalize();
@@ -489,7 +489,7 @@ AttitudeEstimatorQ::update(float dt)
// Accelerometer correction
// Project 'k' unit vector of earth frame to body frame
// Vector3f k = _q.conjugate_inversed(Vector3f(0.0f, 0.0f, 1.0f));
// Vector3f k = _q.rotateVectorInverse(Vector3f(0.0f, 0.0f, 1.0f));
// Optimized version with dropped zeros
Vector3f k(
2.0f * (_q(1) * _q(3) - _q(0) * _q(2)),
src/modules/commander/accelerometer_calibration.cpp
+1 -1
View File
@@ -561,7 +561,7 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
// use vehicle_attitude if available
const vehicle_attitude_s &att = attitude_sub.get();
const matrix::Quatf q{att.q};
const Vector3f accel_ref = q.conjugate_inversed(Vector3f{0.f, 0.f, -CONSTANTS_ONE_G});
const Vector3f accel_ref = q.rotateVectorInverse(Vector3f{0.f, 0.f, -CONSTANTS_ONE_G});
// sanity check angle between acceleration vectors
const float angle = AxisAnglef(Quatf(accel_avg, accel_ref)).angle();
src/modules/uuv_pos_control/uuv_pos_control.cpp
+2 -2
View File
@@ -122,7 +122,7 @@ void UUVPOSControl::pose_controller_6dof(const float x_pos_des, const float y_po
_param_pose_gain_y.get() * (pos_des(1) - vlocal_pos.y) - _param_pose_gain_d_y.get() * vlocal_pos.vy,
_param_pose_gain_z.get() * (pos_des(2) - vlocal_pos.z) - _param_pose_gain_d_z.get() * vlocal_pos.vz);
Vector3f rotated_input = q_att.conjugate_inversed(p_control_output);//rotate the coord.sys (from global to body)
Vector3f rotated_input = q_att.rotateVectorInverse(p_control_output);//rotate the coord.sys (from global to body)
publish_attitude_setpoint(rotated_input(0),
rotated_input(1),
@@ -143,7 +143,7 @@ void UUVPOSControl::stabilization_controller_6dof(const float x_pos_des, const f
0,
_param_pose_gain_z.get() * (pos_des(2) - vlocal_pos.z));
//potential d controller missing
Vector3f rotated_input = q_att.conjugate_inversed(p_control_output);//rotate the coord.sys (from global to body)
Vector3f rotated_input = q_att.rotateVectorInverse(p_control_output);//rotate the coord.sys (from global to body)
publish_attitude_setpoint(rotated_input(0) + x_pos_des, rotated_input(1) + y_pos_des, rotated_input(2),
roll_des, pitch_des, yaw_des);