fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-28 22:44:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

PositionControl: addess @bkueng 's comment in #11056

Browse Source
This commit is contained in:
Matthias Grob 2019-01-08 16:05:43 +01:00
parent f9ec0cd5ea
commit 573dd89cbf
2 changed files with 7 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -55,24 +55,17 @@ void PositionControl::updateState(const PositionControlStates &states)
_vel_dot = states.acceleration;
}
void PositionControl::_setCtrlFlagTrue()
void PositionControl::_setCtrlFlag(bool value)
{
for (int i = 0; i <= 2; i++) {
_ctrl_pos[i] = _ctrl_vel[i] = true;
}
}
void PositionControl::_setCtrlFlagFalse()
{
for (int i = 0; i <= 2; i++) {
_ctrl_pos[i] = _ctrl_vel[i] = false;
_ctrl_pos[i] = _ctrl_vel[i] = value;
}
}
bool PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &setpoint)
{
// Only for logging purpose: by default we use the entire position-velocity control-loop pipeline
_setCtrlFlagTrue();
// by default we use the entire position-velocity control-loop pipeline (flag only for logging purpose)
_setCtrlFlag(true);
_pos_sp = Vector3f(setpoint.x, setpoint.y, setpoint.z);
_vel_sp = Vector3f(setpoint.vx, setpoint.vy, setpoint.vz);
@ -212,8 +205,8 @@ bool PositionControl::_interfaceMapping()
// throttle down such that vehicle goes down with
// 70% of throttle range between min and hover
_thr_sp(2) = -(MPC_THR_MIN.get() + (MPC_THR_HOVER.get() - MPC_THR_MIN.get()) * 0.7f);
// position and velocity control-loop is not used (note: only for logging purpose)
_setCtrlFlagFalse(); // position/velocity control-loop is not used
// position and velocity control-loop is currently unused (flag only for logging purpose)
_setCtrlFlag(false);
}
return !(failsafe);

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

@ -204,8 +204,7 @@ private:
void _positionController(); /** applies the P-position-controller */
void _velocityController(const float &dt); /** applies the PID-velocity-controller */
void _setCtrlFlagTrue(); /**< set control-loop flags to true (only required for logging) */
void _setCtrlFlagFalse(); /**< set control-loop flags to false (only required for logging) */
void _setCtrlFlag(bool value); /**< set control-loop flags (only required for logging) */
matrix::Vector3f _pos{}; /**< MC position */
matrix::Vector3f _vel{}; /**< MC velocity */