dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Multicopter attitude setpoint flag absolute yaw control 

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
msg/vehicle_attitude_setpoint.msg
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 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
float32[4] q_d # Desired quaternion for quaternion control

3
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -246,6 +246,9 @@ MulticopterAttitudeControl::Run()
if (_vehicle_attitude_setpoint_sub.updated()) {
vehicle_attitude_setpoint_s 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);
_thrust_setpoint_body = Vector3f(vehicle_attitude_setpoint.thrust_body);
}

4
src/modules/mc_pos_control/PositionControl/PositionControl.cpp
View File

@ -120,7 +120,6 @@ bool PositionControl::update(const float dt)
_velocityControl(dt);
_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();
}
@ -240,6 +239,7 @@ void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s
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;
}