yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 09:22:18 +00:00
Code Issues Packages Projects Releases Wiki Activity

FlightTasks: move stick handling into library

Browse Source
This commit is contained in:
Matthias Grob
2020-07-14 13:22:47 +02:00
committed by Julian Kent
parent 49a543e1b2
commit 9daff24e79
11 changed files with 202 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
View File

@@ -88,12 +88,12 @@ void FlightTaskManualAltitude::_updateConstraintsFromEstimator()
void FlightTaskManualAltitude::_scaleSticks()
{
// Use stick input with deadzone, exponential curve and first order lpf for yawspeed
const float yawspeed_target = _sticks_expo(3) * math::radians(_param_mpc_man_y_max.get());
const float yawspeed_target = _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get());
_yawspeed_setpoint = _applyYawspeedFilter(yawspeed_target);
// Use sticks input with deadzone and exponential curve for vertical velocity
const float vel_max_z = (_sticks(2) > 0.0f) ? _constraints.speed_down : _constraints.speed_up;
_velocity_setpoint(2) = vel_max_z * _sticks_expo(2);
const float vel_max_z = (_sticks.getPosition()(2) > 0.0f) ? _constraints.speed_down : _constraints.speed_up;
_velocity_setpoint(2) = vel_max_z * _sticks.getPositionExpo()(2);
}
float FlightTaskManualAltitude::_applyYawspeedFilter(float yawspeed_target)
@@ -110,7 +110,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
// If not locked, altitude setpoint is set to NAN.
// Check if user wants to break
const bool apply_brake = fabsf(_sticks_expo(2)) <= FLT_EPSILON;
const bool apply_brake = fabsf(_sticks.getPositionExpo()(2)) <= FLT_EPSILON;
// Check if vehicle has stopped
const bool stopped = (_param_mpc_hold_max_z.get() < FLT_EPSILON || fabsf(_velocity(2)) < _param_mpc_hold_max_z.get());
@@ -122,7 +122,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
float spd_xy = Vector2f(_velocity).length();
// Use presence of horizontal stick inputs as a transition criteria
float stick_xy = Vector2f(&_sticks_expo(0)).length();
float stick_xy = Vector2f(_sticks.getPositionExpo().slice<2, 1>(0, 0)).length();
bool stick_input = stick_xy > 0.001f;
if (_terrain_hold) {
@@ -340,7 +340,7 @@ void FlightTaskManualAltitude::_updateSetpoints()
// thrust along xy is demanded. The maximum thrust along xy depends on the thrust
// setpoint along z-direction, which is computed in PositionControl.cpp.
Vector2f sp(&_sticks(0));
Vector2f sp(_sticks.getPosition().slice<2, 1>(0, 0));
_man_input_filter.setParameters(_deltatime, _param_mc_man_tilt_tau.get());
_man_input_filter.update(sp);
@@ -359,8 +359,8 @@ void FlightTaskManualAltitude::_updateSetpoints()
bool FlightTaskManualAltitude::_checkTakeoff()
{
// stick is deflected above 65% throttle (_sticks(2) is in the range [-1,1])
return _sticks(2) < -0.3f;
// stick is deflected above 65% throttle (throttle stick is in the range [-1,1])
return _sticks.getPosition()(2) < -0.3f;
}
bool FlightTaskManualAltitude::update()