Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Takeoff Revision #12014

Merged
merged 18 commits into from
May 22, 2019
Merged
Changes from 1 commit
Commits
Show all changes
18 commits
Select commit Hold shift + click to select a range
d642c5a
mc_pos_control: refactor smooth takeoff call
MaEtUgR May 11, 2019
d83da14
mc_pos_control: refactor smooth takeoff names
MaEtUgR May 11, 2019
f160902
mc_pos_control: fix smooth takeoff ramp
MaEtUgR May 11, 2019
336f978
mc_pos_control: refactor takeoff trigger conditions to be positive
MaEtUgR May 11, 2019
c38eda4
mc_pos_control: initialize takeoff altitude reference
MaEtUgR May 15, 2019
0c81a19
mc_att_control: fix having high thrust when disarmed
MaEtUgR May 11, 2019
c3d7878
mc_pos_control/FlightTasks: trigger takeoff based on task
MaEtUgR May 13, 2019
5e0bbc3
FlightTasks: fix takeoff trigger for offboard
MaEtUgR May 14, 2019
77562bd
AutoSmoothVel - Override checkTakeoff with task-specific logic and re…
bresch May 14, 2019
ae2588c
mc_pos_control: replace takeoff velocity ramp with thrust ramp
MaEtUgR May 15, 2019
4149871
Add a Takeoff class to handle multicopter takeoff
MaEtUgR May 15, 2019
fa813c0
mc_pos_control: fix adjusting the wrong setpoint
MaEtUgR May 16, 2019
33c7a9c
mc_pos_control: fix updating takeoff state when no flight task is run…
MaEtUgR May 16, 2019
dfe355f
mc_pos_control_params: user friendly default spool/rampup times
MaEtUgR May 16, 2019
0ace048
mc_pos_control: switch back to velocity ramp
MaEtUgR May 17, 2019
78c8b2c
Revert "mc_att_control: fix having high thrust when disarmed"
MaEtUgR May 21, 2019
fce056a
mc_att_control: fix applying not updated thrust setpoint
MaEtUgR May 22, 2019
36736f9
Takeoff: address @RomanBapst's review comments
MaEtUgR May 22, 2019
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
FlightTasks: fix takeoff trigger for offboard
MaEtUgR committed May 16, 2019
commit 5e0bbc3a25fed722b8e3d9e659d083b0a69ca41f
11 changes: 0 additions & 11 deletions src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
Original file line number Diff line number Diff line change
@@ -129,17 +129,6 @@ void FlightTaskAuto::_limitYawRate()
}
}

bool FlightTaskAuto::_checkTakeoff() {
// position setpoint above the minimum altitude
float min_altitude = 0.2f;
const float min_distance_to_ground = _sub_vehicle_local_position->get().hagl_min;
if (PX4_ISFINITE(min_distance_to_ground)) {
min_altitude = min_distance_to_ground + 0.05f;
}

return PX4_ISFINITE(_position_setpoint(2)) && _position_setpoint(2) < (_position(2) - min_altitude);
}

bool FlightTaskAuto::_evaluateTriplets()
{
// TODO: fix the issues mentioned below
1 change: 0 additions & 1 deletion src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
Original file line number Diff line number Diff line change
@@ -91,7 +91,6 @@ class FlightTaskAuto : public FlightTask
matrix::Vector2f _getTargetVelocityXY(); /**< only used for follow-me and only here because of legacy reason.*/
void _updateInternalWaypoints(); /**< Depending on state of vehicle, the internal waypoints might differ from target (for instance if offtrack). */
bool _compute_heading_from_2D_vector(float &heading, matrix::Vector2f v); /**< Computes and sets heading a 2D vector */
virtual bool _checkTakeoff(); /**< determines when to trigger a takeoff (ignored in flight) */

matrix::Vector3f _prev_prev_wp{}; /**< Pre-previous waypoint (local frame). This will be used for smoothing trajectories -> not used yet. */
matrix::Vector3f _prev_wp{}; /**< Previous waypoint (local frame). If no previous triplet is available, the prev_wp is set to current position. */
29 changes: 28 additions & 1 deletion src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp
Original file line number Diff line number Diff line change
@@ -6,7 +6,7 @@ constexpr uint64_t FlightTask::_timeout;
// First index of empty_setpoint corresponds to time-stamp and requires a finite number.
const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}};

const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {}};
const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, false, {}};
const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}};

bool FlightTask::initializeSubscriptions(SubscriptionArray &subscription_array)
@@ -145,3 +145,30 @@ void FlightTask::_setDefaultConstraints()
_constraints.max_distance_to_ground = NAN;
_constraints.want_takeoff = false;
}

bool FlightTask::_checkTakeoff()
{
// position setpoint above the minimum altitude
bool position_triggered_takeoff = false;

if (PX4_ISFINITE(_position_setpoint(2))) {
//minimal altitude either 20cm or what is necessary for correct estimation e.g. optical flow
MaEtUgR marked this conversation as resolved.
Show resolved Hide resolved
float min_altitude = 0.2f;
const float min_distance_to_ground = _sub_vehicle_local_position->get().hagl_min;

if (PX4_ISFINITE(min_distance_to_ground)) {
min_altitude = min_distance_to_ground + 0.05f;
}

position_triggered_takeoff = _position_setpoint(2) < (_position(2) - min_altitude);
}

// upwards velocity setpoint
bool velocity_triggered_takeoff = false;

if (PX4_ISFINITE(_velocity_setpoint(2))) {
velocity_triggered_takeoff = _velocity_setpoint(2) < -0.3f;
}

return position_triggered_takeoff || velocity_triggered_takeoff;
}
15 changes: 6 additions & 9 deletions src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp
Original file line number Diff line number Diff line change
@@ -185,21 +185,18 @@ class FlightTask : public ModuleParams
uORB::Subscription<vehicle_attitude_s> *_sub_attitude{nullptr};
uint8_t _heading_reset_counter{0}; /**< estimator heading reset */

/**
* Reset all setpoints to NAN
*/
/** Reset all setpoints to NAN */
void _resetSetpoints();

/**
* Check and update local position
*/
/** Check and update local position */
void _evaluateVehicleLocalPosition();

/**
* Set constraints to default values
*/
/** Set constraints to default values */
virtual void _setDefaultConstraints();

/** determines when to trigger a takeoff (ignored in flight) */
virtual bool _checkTakeoff();

/* Time abstraction */
static constexpr uint64_t _timeout = 500000; /**< maximal time in us before a loop or data times out */
float _time = 0; /**< passed time in seconds since the task was activated */
Original file line number Diff line number Diff line change
@@ -56,7 +56,7 @@ class FlightTaskManualAltitude : public FlightTaskManual
void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */
virtual void _updateSetpoints(); /**< updates all setpoints */
virtual void _scaleSticks(); /**< scales sticks to velocity in z */
virtual bool _checkTakeoff(); /**< determines when to trigger a takeoff (ignored in flight) */
bool _checkTakeoff() override;

/**
* rotates vector into local frame
15 changes: 7 additions & 8 deletions src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.cpp
Original file line number Diff line number Diff line change
@@ -75,17 +75,16 @@ bool FlightTaskOffboard::activate()

bool FlightTaskOffboard::update()
{
// reset setpoint for every loop
_resetSetpoints();

if (!_sub_triplet_setpoint->get().current.valid) {
_resetSetpoints();
_setDefaultConstraints();
_position_setpoint = _position;
return false;
}

// 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;
@@ -169,7 +168,6 @@ bool FlightTaskOffboard::update()
// 2. position setpoint + velocity setpoint (velocity used as feedforward)
// 3. velocity setpoint
// 4. acceleration setpoint -> this will be mapped to normalized thrust setpoint because acceleration is not supported

const bool position_ctrl_xy = _sub_triplet_setpoint->get().current.position_valid
&& _sub_vehicle_local_position->get().xy_valid;
const bool position_ctrl_z = _sub_triplet_setpoint->get().current.alt_valid
@@ -225,7 +223,6 @@ bool FlightTaskOffboard::update()
}

// Z-direction

if (feedforward_ctrl_z) {
_position_setpoint(2) = _sub_triplet_setpoint->get().current.z;
_velocity_setpoint(2) = _sub_triplet_setpoint->get().current.vz;
@@ -239,12 +236,14 @@ bool FlightTaskOffboard::update()

// Acceleration
// Note: this is not supported yet and will be mapped to normalized thrust directly.

if (_sub_triplet_setpoint->get().current.acceleration_valid) {
_thrust_setpoint(0) = _sub_triplet_setpoint->get().current.a_x;
_thrust_setpoint(1) = _sub_triplet_setpoint->get().current.a_y;
_thrust_setpoint(2) = _sub_triplet_setpoint->get().current.a_z;
}

// use default conditions of upwards position or velocity to take off
_constraints.want_takeoff = _checkTakeoff();

return true;
}