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

FlightTaskOffboard: enable triplet type takeoff, land etc

Browse Source
This commit is contained in:
Dennis Mannhart
2018-07-20 09:26:18 +02:00
committed by Lorenz Meier
parent 1099dd8c7d
commit 740f1f284f
2 changed files with 96 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/FlightTaskOffboard.cpp
+87 -19
View File
@@ -52,6 +52,13 @@ bool FlightTaskOffboard::initializeSubscriptions(SubscriptionArray &subscription
return true;
}
bool FlightTaskOffboard::activate()
{
bool ret = FlightTask::activate();
_position_lock *= NAN;
return ret;
}
bool FlightTaskOffboard::update()
{
if (!_sub_triplet_setpoint->get().current.valid) {
@@ -63,6 +70,86 @@ bool FlightTaskOffboard::update()
// reset setpoint for every loop
_resetSetpoints();
// Yaw / Yaw-speed
if (_sub_triplet_setpoint->get().current.yaw_valid) {
// yaw control required
_yaw_setpoint = _sub_triplet_setpoint->get().current.yaw;
if (_sub_triplet_setpoint->get().current.yawspeed_valid) {
// yawspeed is used as feedforward
_yawspeed_setpoint = _sub_triplet_setpoint->get().current.yawspeed;
}
} else if (_sub_triplet_setpoint->get().current.yawspeed_valid) {
// only yawspeed required
_yawspeed_setpoint = _sub_triplet_setpoint->get().current.yawspeed;
// set yaw setpoint to NAN since not used
_yaw_setpoint = NAN;
}
// Loiter
if (_sub_triplet_setpoint->get().current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
// loiter just means that the vehicle should keep position
if (!PX4_ISFINITE(_position_lock(0))) {
_position_setpoint = _position_lock = _position;
} else {
_position_setpoint = _position_lock;
}
// don't have to continue
return true;
} else {
_position_lock *= NAN;
}
// Takeoff
if (_sub_triplet_setpoint->get().current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
// just do takeoff to default altitude
if (!PX4_ISFINITE(_position_lock(0))) {
_position_setpoint = _position_lock = _position;
_position_setpoint(2) = _position_lock(2) = _position(2) - MIS_TAKEOFF_ALT.get();
} else {
_position_setpoint = _position_lock;
}
// don't have to continue
return true;
} else {
_position_lock *= NAN;
}
// Land
if (_sub_triplet_setpoint->get().current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
// land with landing speed, but keep position in xy
if (!PX4_ISFINITE(_position_lock(0))) {
_position_setpoint = _position_lock = _position;
_position_setpoint(2) = _position_lock(2) = NAN;
_velocity_setpoint(2) = MPC_LAND_SPEED.get();
} else {
_position_setpoint = _position_lock;
_velocity_setpoint(2) = MPC_LAND_SPEED.get();
}
// don't have to continue
return true;
} else {
_position_lock *= NAN;
}
// IDLE
if (_sub_triplet_setpoint->get().current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
_thrust_setpoint.zero();
return true;
}
// Possible inputs:
// 1. position setpoint
// 2. position setpoint + velocity setpoint (velocity used as feedforward)
@@ -136,25 +223,6 @@ bool FlightTaskOffboard::update()
_velocity_setpoint(2) = _sub_triplet_setpoint->get().current.vz;
}
// Yaw / Yaw-speed
if (_sub_triplet_setpoint->get().current.yaw_valid) {
// yaw control required
_yaw_setpoint = _sub_triplet_setpoint->get().current.yaw;
if (_sub_triplet_setpoint->get().current.yawspeed_valid) {
// yawspeed is used as feedforward
_yawspeed_setpoint = _sub_triplet_setpoint->get().current.yawspeed;
}
} else if (_sub_triplet_setpoint->get().current.yawspeed_valid) {
// only yawspeed required
_yawspeed_setpoint = _sub_triplet_setpoint->get().current.yawspeed;
// set yaw setpoint to NAN since not used
_yaw_setpoint = NAN;
}
// Acceleration
// Note: this is not supported yet and will be mapped to normalized thrust directly.
src/lib/FlightTasks/tasks/FlightTaskOffboard.hpp
+9
View File
@@ -49,7 +49,16 @@ public:
virtual ~FlightTaskOffboard() = default;
bool initializeSubscriptions(SubscriptionArray &subscription_array) override;
bool update() override;
bool activate() override;
protected:
uORB::Subscription<position_setpoint_triplet_s> *_sub_triplet_setpoint{nullptr};
private:
matrix::Vector3f _position_lock{};
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
(ParamFloat<px4::params::MPC_LAND_SPEED>) MPC_LAND_SPEED,
(ParamFloat<px4::params::MPC_TKO_SPEED>) MPC_TKO_SPEED,
(ParamFloat<px4::params::MIS_TAKEOFF_ALT>) MIS_TAKEOFF_ALT
)
};