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.

Sign up for GitHub

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

Adjustable land speed via RC #12220

Merged
merged 9 commits into from
Jul 9, 2019
Merged

Adjustable land speed via RC #12220

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
7b88030
Added a subscription to manual_control_setpoint to the FlightTaskAuto…
dakejahl Jun 9, 2019
c9b518a
corrected formatting
dakejahl Jun 9, 2019
fdf53b6
add float literal
dakejahl Jun 12, 2019
ebd7ddf
Fixed calculation of descent speed. Replaced time check with vehicle_…
dakejahl Jun 12, 2019
c1e75ea
Merge branch 'master' into pr-assisted_land_speed
dakejahl Jul 3, 2019
387e1b9
Update parameter description
dakejahl Jul 4, 2019
115c954
Merge branch 'master' into pr-assisted_land_speed
dagar Jul 5, 2019
8081ed6
Merge branch 'master' into pr-assisted_land_speed
bresch Jul 8, 2019
e045510
Merge branch 'master' into pr-assisted_land_speed
dagar Jul 9, 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
4 changes: 4 additions & 0 deletions src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -66,6 +66,10 @@ bool FlightTaskAuto::initializeSubscriptions(SubscriptionArray &subscription_arr
return false;
}

if (!subscription_array.get(ORB_ID(manual_control_setpoint), _sub_manual_control_setpoint)) {
return false;
}

if (!_obstacle_avoidance.initializeSubscriptions(subscription_array)) {
return false;
}
Expand Down
4 changes: 3 additions & 1 deletion src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,7 @@
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/position_setpoint.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <lib/ecl/geo/geo.h>
#include <ObstacleAvoidance.hpp>
Expand Down Expand Up @@ -99,6 +100,8 @@ class FlightTaskAuto : public FlightTask
float _mc_cruise_speed{0.0f}; /**< Requested cruise speed. If not valid, default cruise speed is used. */
WaypointType _type{WaypointType::idle}; /**< Type of current target triplet. */
uORB::SubscriptionPollable<home_position_s> *_sub_home_position{nullptr};
uORB::SubscriptionPollable<manual_control_setpoint_s> *_sub_manual_control_setpoint{nullptr};
uORB::SubscriptionPollable<vehicle_status_s> *_sub_vehicle_status{nullptr};

State _current_state{State::none};
float _target_acceptance_radius = 0.0f; /**< Acceptances radius of the target */
Expand All @@ -122,7 +125,6 @@ class FlightTaskAuto : public FlightTask
matrix::Vector2f _lock_position_xy{NAN, NAN}; /**< if no valid triplet is received, lock positition to current position */
bool _yaw_lock = false; /**< if within acceptance radius, lock yaw to current yaw */
uORB::SubscriptionPollable<position_setpoint_triplet_s> *_sub_triplet_setpoint{nullptr};
uORB::SubscriptionPollable<vehicle_status_s> *_sub_vehicle_status{nullptr};

matrix::Vector3f
_triplet_target; /**< current triplet from navigator which may differ from the intenal one (_target) depending on the vehicle state. */
Expand Down
37 changes: 34 additions & 3 deletions src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -129,11 +129,11 @@ void FlightTaskAutoMapper2::_prepareIdleSetpoints()

void FlightTaskAutoMapper2::_prepareLandSetpoints()
{
float land_speed = _getLandSpeed();

// Keep xy-position and go down with landspeed
_position_setpoint = Vector3f(_target(0), _target(1), NAN);
const float speed_lnd = (_alt_above_ground > _param_mpc_land_alt1.get()) ? _constraints.speed_down :
_param_mpc_land_speed.get();
_velocity_setpoint = Vector3f(Vector3f(NAN, NAN, speed_lnd));
_velocity_setpoint = Vector3f(Vector3f(NAN, NAN, land_speed));

// set constraints
_constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get());
Expand Down Expand Up @@ -195,3 +195,34 @@ bool FlightTaskAutoMapper2::_highEnoughForLandingGear()
// return true if altitude is above two meters
return _alt_above_ground > 2.0f;
}

float FlightTaskAutoMapper2::_getLandSpeed()
{
bool rc_assist_enabled = _param_mpc_land_rc_help.get();
bool rc_is_valid = !_sub_vehicle_status->get().rc_signal_lost;

float throttle = 0.5f;

if (rc_is_valid && rc_assist_enabled) {
throttle = _sub_manual_control_setpoint->get().z;
}

float speed = 0;

if (_alt_above_ground > _param_mpc_land_alt1.get()) {
speed = _constraints.speed_down;

} else {
float land_speed = _param_mpc_land_speed.get();
float head_room = _constraints.speed_down - land_speed;

speed = land_speed + 2 * (0.5f - throttle) * head_room;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't want to offend at all but this implementation is quite hacky and needs improvement for sure.
Issues that come to my mind:

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

No offense taken at all, I like critical feedback. I am pretty unfamiliar with FlightTasks in general, so indeed it may be hacky as I was unsure the correct spot to put this.

We should support general correction of the land location by sticks not only adjusting the land speed.

I've got a WIP branch open right now. My plan was to move the sticks stuff into the FlightTask base class so I could use _evaluateSticks

It's implemented in AutoMapper2 which duplicates big parts of AutoMapper so in some configurations this feature is not available.

I was very confused by the naming of these two. Without getting intimate with the code, it is very unclear what their roles/responsibilities are. I actually implemented it AutoMapper at first and then realized nothing was happening.

If you've got any suggestions or just random details in general don't hesitate to share. Thanks!

Copy link
Member

@MaEtUgR MaEtUgR Oct 21, 2019 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I've got a WIP branch open right now. My plan was to move the sticks stuff into the FlightTask base class so I could use _evaluateSticks

I'm quite sure by now that inheriting all the data processing in the flight tasks does not scale. We should pull e.g. the stick processing into a library such that not every task that uses sticks has to inherit from Manual. (The same of course also for other reusable logic.)

I was very confused by the naming of these two.

That's definitely far from your fault. They do almost completely the same thing but for later auto task with the "AutoSmoothVel" ending (which is default) there are some small differences. We need to get rid of that duplication, here's the issue for that: #11209. The existence of Automapper and having two versions is in my eyes partly caused by the inheritance structure.

Copy link
Member

@MaEtUgR MaEtUgR Feb 19, 2020 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@dakejahl JFYI the duplicate AutoMapper is gone since 7e78eed 🎉

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@dakejahl JFYI the stick handling was moved to a library (#15324) and I have a pr open reusing it for the auto land use case you contributed: #15325

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sweet! Have others found it useful? Haha I never did get around to allowing XY control, it would be cool if it did

Copy link
Member

@MaEtUgR MaEtUgR Jul 15, 2020 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think it doesn't get used that often because of RC override which by default makes it switch out of land when you move the sticks. We'll enable x, y control soon. We can still discuss how it should work together with the mode override.
btw I changed the behavior a bit to not have such a high downwards speed on the sticks. Is that a problem? We can maybe discuss in the dev call...


// Allow minimum assisted land speed to be half of parameter
if (speed < land_speed * 0.5f) {
speed = land_speed * 0.5f;
}
}

return speed;
}
2 changes: 2 additions & 0 deletions src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,7 @@ class FlightTaskAutoMapper2 : public FlightTaskAuto
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto,
(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd,
(ParamInt<px4::params::MPC_LAND_RC_HELP>) _param_mpc_land_rc_help,
(ParamFloat<px4::params::MPC_LAND_ALT1>)
_param_mpc_land_alt1, // altitude at which speed limit downwards reaches maximum speed
(ParamFloat<px4::params::MPC_LAND_ALT2>)
Expand All @@ -80,4 +81,5 @@ class FlightTaskAutoMapper2 : public FlightTaskAuto
void _reset(); /**< Resets member variables to current vehicle state */
WaypointType _type_previous{WaypointType::idle}; /**< Previous type of current target triplet. */
bool _highEnoughForLandingGear(); /**< Checks if gears can be lowered. */
float _getLandSpeed(); /**< Returns landing descent speed. */
};
12 changes: 12 additions & 0 deletions src/modules/mc_pos_control/mc_pos_control_params.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -350,6 +350,18 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 12.0f);
*/
PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 0.7f);

/**
* Enable user assisted descent speed for autonomous land routine.
* When enabled, descent speed will be equal to MPC_LAND_SPEED at half throttle,
* MPC_Z_VEL_MAX_DN at zero throttle, and 0.5 * MPC_LAND_SPEED at full throttle.
*
* @min 0
* @max 1
* @value 0 Fixed descent speed of MPC_LAND_SPEED
* @value 1 User assisted descent speed
*/
PARAM_DEFINE_INT32(MPC_LAND_RC_HELP, 0);

/**
* Takeoff climb rate
*
Expand Down