forked from Archive/PX4-Autopilot
FlightTaskAuto: use new Sticks class for assisted land speed
This commit is contained in:
parent
4366898f11
commit
91c0f19121
|
@ -64,7 +64,6 @@ bool FlightTaskAuto::updateInitialize()
|
|||
bool ret = FlightTask::updateInitialize();
|
||||
|
||||
_sub_home_position.update();
|
||||
_sub_manual_control_setpoint.update();
|
||||
_sub_vehicle_status.update();
|
||||
_sub_triplet_setpoint.update();
|
||||
|
||||
|
|
|
@ -108,7 +108,6 @@ protected:
|
|||
WaypointType _type{WaypointType::idle}; /**< Type of current target triplet. */
|
||||
|
||||
uORB::SubscriptionData<home_position_s> _sub_home_position{ORB_ID(home_position)};
|
||||
uORB::SubscriptionData<manual_control_setpoint_s> _sub_manual_control_setpoint{ORB_ID(manual_control_setpoint)};
|
||||
uORB::SubscriptionData<vehicle_status_s> _sub_vehicle_status{ORB_ID(vehicle_status)};
|
||||
|
||||
State _current_state{State::none};
|
||||
|
|
|
@ -40,6 +40,10 @@
|
|||
|
||||
using namespace matrix;
|
||||
|
||||
FlightTaskAutoMapper::FlightTaskAutoMapper() :
|
||||
_sticks(this)
|
||||
{};
|
||||
|
||||
bool FlightTaskAutoMapper::activate(vehicle_local_position_setpoint_s last_setpoint)
|
||||
{
|
||||
bool ret = FlightTaskAuto::activate(last_setpoint);
|
||||
|
@ -176,15 +180,6 @@ bool FlightTaskAutoMapper::_highEnoughForLandingGear()
|
|||
|
||||
float FlightTaskAutoMapper::_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 (_dist_to_ground > _param_mpc_land_alt1.get()) {
|
||||
|
@ -194,13 +189,18 @@ float FlightTaskAutoMapper::_getLandSpeed()
|
|||
const float land_speed = math::gradual(_dist_to_ground,
|
||||
_param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
|
||||
_param_mpc_land_speed.get(), _constraints.speed_down);
|
||||
const float head_room = _constraints.speed_down - land_speed;
|
||||
|
||||
speed = land_speed + 2 * (0.5f - throttle) * head_room;
|
||||
// user input assisted land speed
|
||||
if (_param_mpc_land_rc_help.get()) {
|
||||
_sticks.evaluateSticks(_time_stamp_current);
|
||||
const float head_room = _constraints.speed_down - land_speed;
|
||||
|
||||
// Allow minimum assisted land speed to be half of parameter
|
||||
if (speed < land_speed * 0.5f) {
|
||||
speed = land_speed * 0.5f;
|
||||
speed = land_speed + _sticks.getPositionExpo()(2) * head_room;
|
||||
|
||||
// Allow minimum assisted land speed to be half of parameter
|
||||
if (speed < land_speed * 0.5f) {
|
||||
speed = land_speed * 0.5f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -41,11 +41,12 @@
|
|||
#pragma once
|
||||
|
||||
#include "FlightTaskAuto.hpp"
|
||||
#include "Sticks.hpp"
|
||||
|
||||
class FlightTaskAutoMapper : public FlightTaskAuto
|
||||
{
|
||||
public:
|
||||
FlightTaskAutoMapper() = default;
|
||||
FlightTaskAutoMapper();
|
||||
virtual ~FlightTaskAutoMapper() = default;
|
||||
bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
|
||||
bool update() override;
|
||||
|
@ -76,7 +77,7 @@ protected:
|
|||
);
|
||||
|
||||
private:
|
||||
|
||||
Sticks _sticks;
|
||||
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. */
|
||||
|
|
Loading…
Reference in New Issue