Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 10:26:52 +08:00
Code Issues Actions 6 Packages Projects Releases Wiki Activity

Multicopter attitude setpoint flag absolute yaw control

Browse Source 
This allows to disable absolute yaw control while still using
the attitude quaternion.
This commit is contained in:
Matthias Grob
2020-10-21 08:37:19 +02:00
parent 0b74076265
commit 3684cfee74
3 changed files with 7 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/vehicle_attitude_setpoint.msg
+2 -1
View File
@@ -4,7 +4,8 @@ float32 roll_body # body angle in NED frame (can be NaN for FW)
float32 pitch_body # body angle in NED frame (can be NaN for FW) float32 pitch_body # body angle in NED frame (can be NaN for FW)
float32 yaw_body # body angle in NED frame (can be NaN for FW) float32 yaw_body # body angle in NED frame (can be NaN for FW)
float32 yaw_sp_move_rate # rad/s (commanded by user) float32 yaw_sp_move_rate # feed-forward NED yaw angular rate in rad/s
bool absolute_heading_valid # True if absolute heading of attitude quaternion should be tracked
# For quaternion-based attitude control # For quaternion-based attitude control
float32[4] q_d # Desired quaternion for quaternion control float32[4] q_d # Desired quaternion for quaternion control
src/modules/mc_att_control/mc_att_control_main.cpp
+3
View File
@@ -246,6 +246,9 @@ MulticopterAttitudeControl::Run()
if (_vehicle_attitude_setpoint_sub.updated()) { if (_vehicle_attitude_setpoint_sub.updated()) {
vehicle_attitude_setpoint_s vehicle_attitude_setpoint; vehicle_attitude_setpoint_s vehicle_attitude_setpoint;
_vehicle_attitude_setpoint_sub.update(&vehicle_attitude_setpoint); _vehicle_attitude_setpoint_sub.update(&vehicle_attitude_setpoint);
const float yaw_weight = vehicle_attitude_setpoint.absolute_heading_valid ? _param_mc_yaw_weight.get() : 0.f;
_attitude_control.setProportionalGain(Vector3f(_param_mc_roll_p.get(), _param_mc_pitch_p.get(), _param_mc_yaw_p.get()),
yaw_weight);
_attitude_control.setAttitudeSetpoint(Quatf(vehicle_attitude_setpoint.q_d), vehicle_attitude_setpoint.yaw_sp_move_rate); _attitude_control.setAttitudeSetpoint(Quatf(vehicle_attitude_setpoint.q_d), vehicle_attitude_setpoint.yaw_sp_move_rate);
_thrust_setpoint_body = Vector3f(vehicle_attitude_setpoint.thrust_body); _thrust_setpoint_body = Vector3f(vehicle_attitude_setpoint.thrust_body);
} }
src/modules/mc_pos_control/PositionControl/PositionControl.cpp
+2 -2
View File
@@ -120,7 +120,6 @@ bool PositionControl::update(const float dt)
_velocityControl(dt); _velocityControl(dt);
_yawspeed_sp = PX4_ISFINITE(_yawspeed_sp) ? _yawspeed_sp : 0.f; _yawspeed_sp = PX4_ISFINITE(_yawspeed_sp) ? _yawspeed_sp : 0.f;
_yaw_sp = PX4_ISFINITE(_yaw_sp) ? _yaw_sp : _yaw; // TODO: better way to disable yaw control
return valid && _updateSuccessful(); return valid && _updateSuccessful();
} }
@@ -240,6 +239,7 @@ void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s
void PositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const void PositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const
{ {
ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, attitude_setpoint); ControlMath::thrustToAttitude(_thr_sp, PX4_ISFINITE(_yaw_sp) ? _yaw_sp : _yaw, attitude_setpoint);
attitude_setpoint.absolute_heading_valid = PX4_ISFINITE(_yaw_sp);
attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp; attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp;
} }