Skip to content
Snippets Groups Projects
Commit f99471f3 authored by Matthias Grob's avatar Matthias Grob Committed by Beat Küng
Browse files

PositionControl: refactor remove Data define for Vector

This define results in super unreadable code.
parent 2405baa2
No related branches found
No related tags found
No related merge requests found
...@@ -50,8 +50,6 @@ ...@@ -50,8 +50,6 @@
using namespace matrix; using namespace matrix;
using Data = matrix::Vector3f;
PositionControl::PositionControl() PositionControl::PositionControl()
{ {
_Pz_h = param_find("MPC_Z_P"); _Pz_h = param_find("MPC_Z_P");
...@@ -73,20 +71,20 @@ PositionControl::PositionControl() ...@@ -73,20 +71,20 @@ PositionControl::PositionControl()
_setParams(); _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); _pos = Vector3f(&state.x);
_vel = Data(&state.vx); _vel = Vector3f(&state.vx);
_yaw = state.yaw; _yaw = state.yaw;
_vel_dot = vel_dot; _vel_dot = vel_dot;
} }
void PositionControl::updateSetpoint(struct vehicle_local_position_setpoint_s setpoint) void PositionControl::updateSetpoint(struct vehicle_local_position_setpoint_s setpoint)
{ {
_pos_sp = Data(&setpoint.x); _pos_sp = Vector3f(&setpoint.x);
_vel_sp = Data(&setpoint.vx); _vel_sp = Vector3f(&setpoint.vx);
_acc_sp = Data(&setpoint.acc_x); _acc_sp = Vector3f(&setpoint.acc_x);
_thr_sp = Data(setpoint.thrust); _thr_sp = Vector3f(setpoint.thrust);
_yaw_sp = setpoint.yaw; _yaw_sp = setpoint.yaw;
_yawspeed_sp = setpoint.yawspeed; _yawspeed_sp = setpoint.yawspeed;
_interfaceMapping(); _interfaceMapping();
...@@ -204,12 +202,12 @@ void PositionControl::_velocityController(const float &dt) ...@@ -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 /* TODO: add offboard acceleration mode
* PID-controller */ * PID-controller */
Data offset(0.0f, 0.0f, _ThrHover); Vector3f offset(0.0f, 0.0f, _ThrHover);
Data thr_sp = Pv.emult(vel_err) + Dv.emult(_vel_dot) + _thr_int - offset; Vector3f thr_sp = Pv.emult(vel_err) + Dv.emult(_vel_dot) + _thr_int - offset;
/* Get maximum tilt */ /* Get maximum tilt */
float tilt_max = PX4_ISFINITE(_constraints.tilt_max) ? _constraints.tilt_max : M_PI_2_F; float tilt_max = PX4_ISFINITE(_constraints.tilt_max) ? _constraints.tilt_max : M_PI_2_F;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment