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 @@
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;
......
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