fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-05 03:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

PositionControl: refactor remove Data define for Vector

Browse Source 
This define results in super unreadable code.
This commit is contained in:
Matthias Grob
2018-03-06 18:57:09 +00:00
committed by Beat Küng
parent 2405baa2c9
commit f99471f34c
1 changed files with 10 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/PositionControl.cpp
+10 -12
View File
@@ -50,8 +50,6 @@
using namespace matrix;
using Data = matrix::Vector3f;
PositionControl::PositionControl()
{
_Pz_h = param_find("MPC_Z_P");
@@ -73,20 +71,20 @@ PositionControl::PositionControl()
_setParams();
};
void PositionControl::updateState(const struct vehicle_local_position_s state, const Data &vel_dot)
void PositionControl::updateState(const struct vehicle_local_position_s state, const Vector3f &vel_dot)
{
_pos = Data(&state.x);
_vel = Data(&state.vx);
_pos = Vector3f(&state.x);
_vel = Vector3f(&state.vx);
_yaw = state.yaw;
_vel_dot = vel_dot;
}
void PositionControl::updateSetpoint(struct vehicle_local_position_setpoint_s setpoint)
{
_pos_sp = Data(&setpoint.x);
_vel_sp = Data(&setpoint.vx);
_acc_sp = Data(&setpoint.acc_x);
_thr_sp = Data(setpoint.thrust);
_pos_sp = Vector3f(&setpoint.x);
_vel_sp = Vector3f(&setpoint.vx);
_acc_sp = Vector3f(&setpoint.acc_x);
_thr_sp = Vector3f(setpoint.thrust);
_yaw_sp = setpoint.yaw;
_yawspeed_sp = setpoint.yawspeed;
_interfaceMapping();
@@ -204,12 +202,12 @@ void PositionControl::_velocityController(const float &dt)
*
*/
Data vel_err = _vel_sp - _vel;
Vector3f vel_err = _vel_sp - _vel;
/* TODO: add offboard acceleration mode
* PID-controller */
Data offset(0.0f, 0.0f, _ThrHover);
Data thr_sp = Pv.emult(vel_err) + Dv.emult(_vel_dot) + _thr_int - offset;
Vector3f offset(0.0f, 0.0f, _ThrHover);
Vector3f thr_sp = Pv.emult(vel_err) + Dv.emult(_vel_dot) + _thr_int - offset;
/* Get maximum tilt */
float tilt_max = PX4_ISFINITE(_constraints.tilt_max) ? _constraints.tilt_max : M_PI_2_F;