Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 11:59:17 +08:00
Code Issues Actions 20 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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/PositionControl.cpp
+6 -13
View File
@@ -55,24 +55,17 @@ void PositionControl::updateState(const PositionControlStates &states)
_vel_dot = states.acceleration; _vel_dot = states.acceleration;
} }
void PositionControl::_setCtrlFlagTrue() void PositionControl::_setCtrlFlag(bool value)
{ {
for (int i = 0; i <= 2; i++) { for (int i = 0; i <= 2; i++) {
_ctrl_pos[i] = _ctrl_vel[i] = true; _ctrl_pos[i] = _ctrl_vel[i] = value;
}
}
void PositionControl::_setCtrlFlagFalse()
{
for (int i = 0; i <= 2; i++) {
_ctrl_pos[i] = _ctrl_vel[i] = false;
} }
} }
bool PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &setpoint) 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 // by default we use the entire position-velocity control-loop pipeline (flag only for logging purpose)
_setCtrlFlagTrue(); _setCtrlFlag(true);
_pos_sp = Vector3f(setpoint.x, setpoint.y, setpoint.z); _pos_sp = Vector3f(setpoint.x, setpoint.y, setpoint.z);
_vel_sp = Vector3f(setpoint.vx, setpoint.vy, setpoint.vz); _vel_sp = Vector3f(setpoint.vx, setpoint.vy, setpoint.vz);
@@ -212,8 +205,8 @@ bool PositionControl::_interfaceMapping()
// throttle down such that vehicle goes down with // throttle down such that vehicle goes down with
// 70% of throttle range between min and hover // 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); _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) // position and velocity control-loop is currently unused (flag only for logging purpose)
_setCtrlFlagFalse(); // position/velocity control-loop is not used _setCtrlFlag(false);
} }
return !(failsafe); return !(failsafe);
src/modules/mc_pos_control/PositionControl.hpp
+1 -2
View File
@@ -204,8 +204,7 @@ private:
void _positionController(); /** applies the P-position-controller */ void _positionController(); /** applies the P-position-controller */
void _velocityController(const float &dt); /** applies the PID-velocity-controller */ void _velocityController(const float &dt); /** applies the PID-velocity-controller */
void _setCtrlFlagTrue(); /**< set control-loop flags to true (only required for logging) */ void _setCtrlFlag(bool value); /**< set control-loop flags (only required for logging) */
void _setCtrlFlagFalse(); /**< set control-loop flags to false (only required for logging) */
matrix::Vector3f _pos{}; /**< MC position */ matrix::Vector3f _pos{}; /**< MC position */
matrix::Vector3f _vel{}; /**< MC velocity */ matrix::Vector3f _vel{}; /**< MC velocity */