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: adapt to acceleration based control

Browse Source
This commit is contained in:
Matthias Grob
2020-01-27 17:17:43 +01:00
parent b3d7445059
commit b79b095ce7
10 changed files with 38 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -38,6 +38,7 @@
#include "FlightTaskManualAltitude.hpp"
#include <float.h>
#include <mathlib/mathlib.h>
#include <ecl/geo/geo.h>
using namespace matrix;
@@ -53,10 +54,10 @@ bool FlightTaskManualAltitude::activate(vehicle_local_position_setpoint_s last_s
{
bool ret = FlightTaskManual::activate(last_setpoint);
_yaw_setpoint = NAN;
_yawspeed_setpoint = 0.0f;
_thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, NAN); // altitude is controlled from position/velocity
_yawspeed_setpoint = 0.f;
_acceleration_setpoint = Vector3f(0.f, 0.f, NAN); // altitude is controlled from position/velocity
_position_setpoint(2) = _position(2);
_velocity_setpoint(2) = 0.0f;
_velocity_setpoint(2) = 0.f;
_setDefaultConstraints();
_constraints.tilt = math::radians(_param_mpc_man_tilt_max.get());
@@ -348,7 +349,7 @@ void FlightTaskManualAltitude::_updateSetpoints()
sp.normalize();
}
_thrust_setpoint.xy() = sp;
_acceleration_setpoint.xy() = sp * tanf(math::radians(_param_mpc_man_tilt_max.get())) * CONSTANTS_ONE_G;
_updateAltitudeLock();
_respectGroundSlowdown();