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

PositionControl: add getter for output position setpoint

This commit is contained in:
Matthias Grob 2019-10-22 18:38:47 +02:00
parent 9e309f62a9
commit 198605d258
3 changed files with 21 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
* Copyright (c) 2018 - 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -351,6 +351,20 @@ void PositionControl::updateConstraints(const vehicle_constraints_s &constraints
}
}
void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s &local_position_setpoint)
{
local_position_setpoint.x = _pos_sp(0);
local_position_setpoint.y = _pos_sp(1);
local_position_setpoint.z = _pos_sp(2);
local_position_setpoint.yaw = _yaw_sp;
local_position_setpoint.yawspeed = _yawspeed_sp;
local_position_setpoint.vx = _vel_sp(0);
local_position_setpoint.vy = _vel_sp(1);
local_position_setpoint.vz = _vel_sp(2);
_acc_sp.copyTo(local_position_setpoint.acceleration);
_thr_sp.copyTo(local_position_setpoint.thrust);
}
void PositionControl::updateParams()
{
ModuleParams::updateParams();

37
src/modules/mc_pos_control/PositionControl/PositionControl.hpp
View File

@ -128,20 +128,6 @@ public:
*/
void resetIntegralZ() { _thr_int(2) = 0.0f; }
/**
* Get the
* @see _thr_sp
* @return The thrust set-point member.
*/
const matrix::Vector3f &getThrustSetpoint() { return _thr_sp; }
/**
* Get the
* @see _yaw_sp
* @return The yaw set-point member.
*/
const float &getYawSetpoint() { return _yaw_sp; }
/**
* Get the
* @see _yawspeed_sp
@ -171,25 +157,12 @@ public:
}
/**
* Get the
* @see _pos_sp
* @return The position set-point that was executed in the control-loop. Nan if the position control-loop was skipped.
* Get the controllers output local position setpoint
* These setpoints are the ones which were executed on including PID output and feed-forward.
* The acceleration or thrust setpoints can be used for attitude control.
* @param local_position_setpoint reference to struct to fill up
*/
const matrix::Vector3f getPosSp()
{
matrix::Vector3f pos_sp{};
for (int i = 0; i <= 2; i++) {
if (_ctrl_pos[i]) {
pos_sp(i) = _pos_sp(i);
} else {
pos_sp(i) = NAN;
}
}
return pos_sp;
}
void getLocalPositionSetpoint(vehicle_local_position_setpoint_s &local_position_setpoint);
protected:

7
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -668,15 +668,10 @@ MulticopterPositionControl::Run()
// PositionController.
vehicle_local_position_setpoint_s local_pos_sp{};
local_pos_sp.timestamp = hrt_absolute_time();
local_pos_sp.x = setpoint.x;
local_pos_sp.y = setpoint.y;
local_pos_sp.z = setpoint.z;
local_pos_sp.yaw = _control.getYawSetpoint();
local_pos_sp.yawspeed = _control.getYawspeedSetpoint();
_control.getLocalPositionSetpoint(local_pos_sp);
local_pos_sp.vx = PX4_ISFINITE(_control.getVelSp()(0)) ? _control.getVelSp()(0) : setpoint.vx;
local_pos_sp.vy = PX4_ISFINITE(_control.getVelSp()(1)) ? _control.getVelSp()(1) : setpoint.vy;
local_pos_sp.vz = PX4_ISFINITE(_control.getVelSp()(2)) ? _control.getVelSp()(2) : setpoint.vz;
_control.getThrustSetpoint().copyTo(local_pos_sp.thrust);
// Publish local position setpoint
// This message will be used by other modules (such as Landdetector) to determine