fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-05 07:10:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

PositionControl: add control states structure

Browse Source
This commit is contained in:
Dennis Mannhart
2018-04-30 17:56:42 +02:00
committed by Lorenz Meier
parent bf15e852ff
commit 9c2ec3c229
2 changed files with 25 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/PositionControl.cpp
+16 -6
View File
@@ -46,12 +46,12 @@ PositionControl::PositionControl(ModuleParams *parent) :
ModuleParams(parent)
{}
void PositionControl::updateState(const vehicle_local_position_s &state, const Vector3f &vel_dot)
void PositionControl::updateState(const PositionControlStates &states)
{
_pos = Vector3f(&state.x);
_vel = Vector3f(&state.vx);
_yaw = state.yaw;
_vel_dot = vel_dot;
_pos = states.position;
_vel = states.velocity;
_yaw = states.yaw;
_vel_dot = states.acceleration;
}
void PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &setpoint)
@@ -119,6 +119,11 @@ void PositionControl::_interfaceMapping()
// thrust setpoint is not supported in position control
_thr_sp(i) = 0.0f;
// to run position control, we require valid position and velocity
if (!PX4_ISFINITE(_pos(i)) || !PX4_ISFINITE(_vel(i))) {
failsafe = true;
}
} else if (PX4_ISFINITE(_vel_sp(i))) {
// Velocity controller is active without position control.
@@ -128,6 +133,11 @@ void PositionControl::_interfaceMapping()
// thrust setpoint is not supported in position control
_thr_sp(i) = 0.0f;
// to run velocity control, we require valid velocity
if (!PX4_ISFINITE(_vel(i))) {
failsafe = true;
}
} else if (PX4_ISFINITE(_thr_sp(i))) {
// Thrust setpoint was generated from sticks directly.
@@ -142,7 +152,6 @@ void PositionControl::_interfaceMapping()
} else {
// nothing is valid. do failsafe
failsafe = true;
PX4_WARN("TODO: add safety");
}
}
@@ -160,6 +169,7 @@ void PositionControl::_interfaceMapping()
// check failsafe
if (failsafe) {
_skip_controller = true;
// point the thrust upwards
_thr_sp(0) = _thr_sp(1) = 0.0f;
// throttle down such that vehicle goes down with