fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 01:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

vehicle_local_position: rename yaw -> heading and add reset logic

Browse Source 
- vehicle_global_position yaw removed (redundant)
This commit is contained in:
Daniel Agar
2020-08-10 11:42:03 +02:00
committed by Mathieu Bresciani
parent 33fb9d0c62
commit 97fc1db768
24 changed files with 75 additions and 86 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/PositionControl/ControlMath.cpp
+7 -7
View File
@@ -77,7 +77,7 @@ void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpo
body_z.normalize();
// vector of desired yaw direction in XY plane, rotated by PI/2
Vector3f y_C(-sinf(yaw_sp), cosf(yaw_sp), 0.0f);
const Vector3f y_C{-sinf(yaw_sp), cosf(yaw_sp), 0.f};
// desired body_x axis, orthogonal to body_z
Vector3f body_x = y_C % body_z;
@@ -97,7 +97,7 @@ void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpo
body_x.normalize();
// desired body_y axis
Vector3f body_y = body_z % body_x;
const Vector3f body_y = body_z % body_x;
Dcmf R_sp;
@@ -109,14 +109,14 @@ void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpo
}
// copy quaternion setpoint to attitude setpoint topic
Quatf q_sp = R_sp;
const Quatf q_sp{R_sp};
q_sp.copyTo(att_sp.q_d);
// calculate euler angles, for logging only, must not be used for control
Eulerf euler = R_sp;
att_sp.roll_body = euler(0);
att_sp.pitch_body = euler(1);
att_sp.yaw_body = euler(2);
const Eulerf euler{R_sp};
att_sp.roll_body = euler.phi();
att_sp.pitch_body = euler.theta();
att_sp.yaw_body = euler.psi();
}
Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max)
src/modules/mc_pos_control/mc_pos_control_main.cpp
+4 -9
View File
@@ -121,7 +121,6 @@ private:
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */
uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)}; /**< vehicle attitude */
uORB::Subscription _home_pos_sub{ORB_ID(home_position)}; /**< home position */
uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
@@ -423,14 +422,6 @@ MulticopterPositionControl::poll_subscriptions()
_control_mode_sub.update(&_control_mode);
_home_pos_sub.update(&_home_pos);
if (_att_sub.updated()) {
vehicle_attitude_s att;
if (_att_sub.copy(&att) && PX4_ISFINITE(att.q[0])) {
_states.yaw = Eulerf(Quatf(att.q)).psi();
}
}
if (_param_mpc_use_hte.get()) {
hover_thrust_estimate_s hte;
@@ -514,6 +505,10 @@ MulticopterPositionControl::set_vehicle_states(const float &vel_sp_z)
// reset derivative to prevent acceleration spikes when regaining velocity
_vel_z_deriv.reset();
}
if (PX4_ISFINITE(_local_pos.heading)) {
_states.yaw = _local_pos.heading;
}
}
int