From d6fe2159ae4cca1d7563f91cb29b452034fefe04 Mon Sep 17 00:00:00 2001
From: Dennis Mannhart <dennis.mannhart@gmail.com>
Date: Thu, 15 Feb 2018 11:13:32 +0100
Subject: [PATCH] FlightTaskManualPosition: remove update method and old member
variable setpoints
---
.../tasks/FlightTaskManualPosition.cpp | 48 +++++++------------
.../tasks/FlightTaskManualPosition.hpp | 11 ++---
2 files changed, 21 insertions(+), 38 deletions(-)
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp
index 1b755b21b8..a5c9bc5176 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2017 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -50,15 +50,14 @@ FlightTaskManualPosition::FlightTaskManualPosition(control::SuperBlock *parent,
bool FlightTaskManualPosition::activate()
{
- _pos_sp_xy = matrix::Vector2f(NAN, NAN);
- _vel_sp_xy = matrix::Vector2f(0.0f, 0.0f);
- return FlightTaskManualAltitude::activate();
+ bool ret = FlightTaskManualAltitude::activate();
+ _vel_sp(0) = _vel_sp(1) = 0.0f;
+ return ret;
}
void FlightTaskManualPosition::_scaleSticks()
{
- /* Use same scaling as for FlightTaskManualAltitude to
- * get yaw and z */
+ /* Use same scaling as for FlightTaskManualAltitude */
FlightTaskManualAltitude::_scaleSticks();
/* Constrain length of stick inputs to 1 for xy*/
@@ -71,47 +70,36 @@ void FlightTaskManualPosition::_scaleSticks()
}
/* Scale to velocity.*/
- _vel_sp_xy = stick_xy * _vel_xy_manual_max.get();
+ matrix::Vector2f vel_sp_xy = stick_xy * _vel_xy_manual_max.get();
/* Rotate setpoint into local frame. */
matrix::Quatf q_yaw = matrix::AxisAnglef(matrix::Vector3f(0.0f, 0.0f, 1.0f), _yaw);
- matrix::Vector3f vel_world = q_yaw.conjugate(matrix::Vector3f(_vel_sp_xy(0), _vel_sp_xy(1), 0.0f));
- _vel_sp_xy = matrix::Vector2f(vel_world(0), vel_world(1));
+ matrix::Vector3f vel_world = q_yaw.conjugate(matrix::Vector3f(vel_sp_xy(0), vel_sp_xy(1), 0.0f));
+ _vel_sp(0) = vel_world(0);
+ _vel_sp(1) = vel_world(1);
}
void FlightTaskManualPosition::_updateXYlock()
{
/* If position lock is not active, position setpoint is set to NAN.*/
const float vel_xy_norm = Vector2f(&_velocity(0)).length();
- const bool apply_brake = _vel_sp_xy.length() < FLT_EPSILON;
+ const bool apply_brake = Vector2f(&_vel_sp(0)).length() < FLT_EPSILON;
const bool stopped = (_vel_hold_thr_xy.get() < FLT_EPSILON || vel_xy_norm < _vel_hold_thr_xy.get());
- if (apply_brake && stopped && !PX4_ISFINITE(_pos_sp_xy(0))) {
- _pos_sp_xy = matrix::Vector2f(&_position(0));
+ if (apply_brake && stopped && !PX4_ISFINITE(_pos_sp(0))) {
+ _pos_sp(0) = _position(0);
+ _pos_sp(1) = _position(1);
} else if (!apply_brake) {
- /* Don't use position setpoint */
- _pos_sp_xy = _pos_sp_xy * NAN;
+ /* don't lock*/
+ _pos_sp(0) = NAN;
+ _pos_sp(1) = NAN;
}
}
void FlightTaskManualPosition::_updateSetpoints()
{
- FlightTaskManualAltitude::_updateSetpoints(); // get yaw and z setpoints
+ FlightTaskManualAltitude::_updateSetpoints(); // needed to get yaw and setpoints in z-direction
+ _thr_sp *= NAN; // don't require any thrust setpoints
_updateXYlock(); // check for position lock
}
-
-bool FlightTaskManualPosition::update()
-{
- _scaleSticks();
- _updateSetpoints();
-
- _setPositionSetpoint(Vector3f(_pos_sp_xy(0), _pos_sp_xy(1), _pos_sp_z));
- _setVelocitySetpoint(Vector3f(_vel_sp_xy(0), _vel_sp_xy(1), _vel_sp_z));
- _setYawSetpoint(_yaw_sp);
- _setYawspeedSetpoint(_yaw_rate_sp);
- _setThrustSetpoint(Vector3f(NAN, NAN, NAN));
-
-
- return true;
-}
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp
index 62856287b3..e9a9929e00 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp
@@ -51,17 +51,12 @@ public:
bool activate() override;
- bool update() override;
-
protected:
- matrix::Vector2f _vel_sp_xy{}; /**< Scaled velocity setpoint from stick. NAN during position lock */
- matrix::Vector2f _pos_sp_xy{}; /**< Position setpoint during lock. Otherwise NAN */
-
- control::BlockParamFloat _vel_xy_manual_max; /**< Maximum speed allowed horizontally */
- control::BlockParamFloat _acc_xy_max;/**< Maximum acceleration horizontally. Only used to compute lock time */
+ control::BlockParamFloat _vel_xy_manual_max; /**< maximum speed allowed horizontally */
+ control::BlockParamFloat _acc_xy_max;/**< maximum acceleration horizontally. Only used to compute lock time */
control::BlockParamFloat _vel_hold_thr_xy; /**< velocity threshold to switch back into horizontal position hold */
- void _updateXYlock(); /**< Applies positon lock based on stick and velocity */
+ void _updateXYlock(); /**< applies positon lock based on stick and velocity */
void _updateSetpoints() override;
void _scaleSticks() override;
};
--
GitLab