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

FlightTasks: adapt to acceleration based control

Browse Source
This commit is contained in:
Matthias Grob
2020-03-30 09:58:39 +02: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
src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.cpp
+8 -9
View File
@@ -41,9 +41,9 @@ bool FlightTaskFailsafe::activate(vehicle_local_position_setpoint_s last_setpoin
bool ret = FlightTask::activate(last_setpoint);
_position_setpoint = _position;
_velocity_setpoint.zero();
_thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, -_param_mpc_thr_hover.get() * 0.6f);
_acceleration_setpoint = matrix::Vector3f(0.f, 0.f, .3f);
_yaw_setpoint = _yaw;
_yawspeed_setpoint = 0.0f;
_yawspeed_setpoint = 0.f;
return ret;
}
@@ -51,27 +51,26 @@ bool FlightTaskFailsafe::update()
{
if (PX4_ISFINITE(_position(0)) && PX4_ISFINITE(_position(1))) {
// stay at current position setpoint
_velocity_setpoint(0) = _velocity_setpoint(1) = 0.0f;
_thrust_setpoint(0) = _thrust_setpoint(1) = 0.0f;
_velocity_setpoint(0) = _velocity_setpoint(1) = 0.f;
_acceleration_setpoint(0) = _acceleration_setpoint(1) = 0.f;
} else if (PX4_ISFINITE(_velocity(0)) && PX4_ISFINITE(_velocity(1))) {
// don't move along xy
_position_setpoint(0) = _position_setpoint(1) = NAN;
_thrust_setpoint(0) = _thrust_setpoint(1) = NAN;
_acceleration_setpoint(0) = _acceleration_setpoint(1) = NAN;
}
if (PX4_ISFINITE(_position(2))) {
// stay at current altitude setpoint
_velocity_setpoint(2) = 0.0f;
_thrust_setpoint(2) = NAN;
_velocity_setpoint(2) = 0.f;
_acceleration_setpoint(2) = NAN;
} else if (PX4_ISFINITE(_velocity(2))) {
// land with landspeed
_velocity_setpoint(2) = _param_mpc_land_speed.get();
_position_setpoint(2) = NAN;
_thrust_setpoint(2) = NAN;
_acceleration_setpoint(2) = NAN;
}
return true;
}
src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.hpp
+1 -3
View File
@@ -51,8 +51,6 @@ public:
private:
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
(ParamFloat<px4::params::MPC_THR_HOVER>)
_param_mpc_thr_hover /**< throttle value at which vehicle is at hover equilibrium */
(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed
)
};