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

mc_attitude_control: keep last attitude setpoint as member

Browse Source 
The last attitude setpoint that is known from the position controller
is now kept inside the AttitudeControl class such that we don't
keep the whole vehicle_attitude_setpoint struct and always copy over from
there to run an update step.
This commit is contained in:
Matthias Grob
2020-05-26 15:07:45 +03:00
committed by Roman Bapst
parent bd154bf33c
commit cf658494ad
5 changed files with 42 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/mc_att_control_main.cpp
+9 -6
View File
@@ -69,7 +69,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl(bool vtol) :
/* initialize quaternions in messages to be valid */
_v_att.q[0] = 1.f;
_v_att_sp.q_d[0] = 1.f;
parameters_updated();
}
@@ -234,8 +233,14 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_
void
MulticopterAttitudeControl::control_attitude()
{
_v_att_sp_sub.update(&_v_att_sp);
_rates_sp = _attitude_control.update(Quatf(_v_att.q), Quatf(_v_att_sp.q_d), _v_att_sp.yaw_sp_move_rate);
if (_vehicle_attitude_setpoint_sub.updated()) {
vehicle_attitude_setpoint_s vehicle_attitude_setpoint;
_vehicle_attitude_setpoint_sub.update(&vehicle_attitude_setpoint);
_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);
}
_rates_sp = _attitude_control.update(Quatf(_v_att.q));
}
void
@@ -246,9 +251,7 @@ MulticopterAttitudeControl::publish_rates_setpoint()
v_rates_sp.roll = _rates_sp(0);
v_rates_sp.pitch = _rates_sp(1);
v_rates_sp.yaw = _rates_sp(2);
v_rates_sp.thrust_body[0] = _v_att_sp.thrust_body[0];
v_rates_sp.thrust_body[1] = _v_att_sp.thrust_body[1];
v_rates_sp.thrust_body[2] = _v_att_sp.thrust_body[2];
_thrust_setpoint_body.copyTo(v_rates_sp.thrust_body);
v_rates_sp.timestamp = hrt_absolute_time();
_v_rates_sp_pub.publish(v_rates_sp);