From 223dacee64342c8d86d8ee054952fc787b6152fa Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 28 Aug 2018 14:57:47 -0400
Subject: [PATCH] multicopter position controller use const references
---
src/lib/FlightTasks/FlightTasks.cpp | 2 +-
src/lib/FlightTasks/FlightTasks.hpp | 2 +-
.../tasks/FlightTask/FlightTask.hpp | 6 +-
.../mc_pos_control/PositionControl.cpp | 18 +++---
.../mc_pos_control/PositionControl.hpp | 16 ++---
.../mc_pos_control/mc_pos_control_main.cpp | 58 +++++++++----------
6 files changed, 49 insertions(+), 53 deletions(-)
diff --git a/src/lib/FlightTasks/FlightTasks.cpp b/src/lib/FlightTasks/FlightTasks.cpp
index f14fe3ee89..d9c4926e97 100644
--- a/src/lib/FlightTasks/FlightTasks.cpp
+++ b/src/lib/FlightTasks/FlightTasks.cpp
@@ -57,7 +57,7 @@ const vehicle_trajectory_waypoint_s FlightTasks::getAvoidanceWaypoint()
}
}
-const vehicle_trajectory_waypoint_s FlightTasks::getEmptyAvoidanceWaypoint()
+const vehicle_trajectory_waypoint_s &FlightTasks::getEmptyAvoidanceWaypoint()
{
return FlightTask::empty_trajectory_waypoint;
}
diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp
index c795aeb55c..d60c6b4e12 100644
--- a/src/lib/FlightTasks/FlightTasks.hpp
+++ b/src/lib/FlightTasks/FlightTasks.hpp
@@ -87,7 +87,7 @@ public:
* Get empty avoidance desired waypoints
* @return empty triplets in the mc_pos_control
*/
- const vehicle_trajectory_waypoint_s getEmptyAvoidanceWaypoint();
+ const vehicle_trajectory_waypoint_s &getEmptyAvoidanceWaypoint();
/**
* Switch to the next task in the available list (for testing)
diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp
index bc4b2a95be..4a5b665bde 100644
--- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp
@@ -81,7 +81,7 @@ public:
* To be called to adopt parameters from an arrived vehicle command
* @return true if accepted, false if declined
*/
- virtual bool applyCommandParameters(const vehicle_command_s &command) { return true; };
+ virtual bool applyCommandParameters(const vehicle_command_s &command) { return true; }
/**
* Call before activate() or update()
@@ -106,13 +106,13 @@ public:
* The constraints can vary with task.
* @return constraints
*/
- const vehicle_constraints_s getConstraints() {return _constraints;};
+ const vehicle_constraints_s &getConstraints() { return _constraints; }
/**
* Get avoidance desired waypoint
* @return desired waypoints
*/
- const vehicle_trajectory_waypoint_s getAvoidanceWaypoint() {return _desired_waypoint;};
+ const vehicle_trajectory_waypoint_s &getAvoidanceWaypoint() { return _desired_waypoint; }
/**
* Empty setpoint.
diff --git a/src/modules/mc_pos_control/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl.cpp
index bb8645cf3f..446df4b6a3 100644
--- a/src/modules/mc_pos_control/PositionControl.cpp
+++ b/src/modules/mc_pos_control/PositionControl.cpp
@@ -60,9 +60,9 @@ void PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &se
// controller and just return thrust.
_skip_controller = false;
- _pos_sp = Vector3f(&setpoint.x);
- _vel_sp = Vector3f(&setpoint.vx);
- _acc_sp = Vector3f(&setpoint.acc_x);
+ _pos_sp = Vector3f(setpoint.x, setpoint.y, setpoint.z);
+ _vel_sp = Vector3f(setpoint.vx, setpoint.vy, setpoint.vz);
+ _acc_sp = Vector3f(setpoint.acc_x, setpoint.acc_y, setpoint.acc_z);
_thr_sp = Vector3f(setpoint.thrust);
_yaw_sp = setpoint.yaw;
_yawspeed_sp = setpoint.yawspeed;
@@ -73,7 +73,7 @@ void PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &se
}
}
-void PositionControl::generateThrustYawSetpoint(const float &dt)
+void PositionControl::generateThrustYawSetpoint(const float dt)
{
if (_skip_controller) {
// Already received a valid thrust set-point.
@@ -214,13 +214,13 @@ void PositionControl::_interfaceMapping()
void PositionControl::_positionController()
{
// P-position controller
- Vector3f vel_sp_position = (_pos_sp - _pos).emult(Vector3f(MPC_XY_P.get(), MPC_XY_P.get(), MPC_Z_P.get()));
+ const Vector3f vel_sp_position = (_pos_sp - _pos).emult(Vector3f(MPC_XY_P.get(), MPC_XY_P.get(), MPC_Z_P.get()));
_vel_sp = vel_sp_position + _vel_sp;
// Constrain horizontal velocity by prioritizing the velocity component along the
// the desired position setpoint over the feed-forward term.
- Vector2f vel_sp_xy = ControlMath::constrainXY(Vector2f(&vel_sp_position(0)),
- Vector2f(&(_vel_sp - vel_sp_position)(0)), _constraints.speed_xy);
+ const Vector2f vel_sp_xy = ControlMath::constrainXY(Vector2f(vel_sp_position(0), vel_sp_position(1)),
+ Vector2f(&(_vel_sp - vel_sp_position)(0)), _constraints.speed_xy);
_vel_sp(0) = vel_sp_xy(0);
_vel_sp(1) = vel_sp_xy(1);
// Constrain velocity in z-direction.
@@ -254,7 +254,7 @@ void PositionControl::_velocityController(const float &dt)
// consideration of the desired thrust in D-direction. In addition, the thrust in
// NE-direction is also limited by the maximum tilt.
- Vector3f vel_err = _vel_sp - _vel;
+ const Vector3f vel_err = _vel_sp - _vel;
// Consider thrust in D-direction.
float thrust_desired_D = MPC_Z_VEL_P.get() * vel_err(2) + MPC_Z_VEL_D.get() * _vel_dot(2) + _thr_int(
@@ -307,7 +307,7 @@ void PositionControl::_velocityController(const float &dt)
}
// Get the direction of (r-y) in NE-direction.
- float direction_NE = Vector2f(&vel_err(0)) * Vector2f(&_vel_sp(0));
+ float direction_NE = Vector2f(vel_err(0), vel_err(1)) * Vector2f(_vel_sp(0), _vel_sp(1));
// Apply Anti-Windup in NE-direction.
bool stop_integral_NE = (thrust_desired_NE * thrust_desired_NE >= thrust_max_NE * thrust_max_NE &&
diff --git a/src/modules/mc_pos_control/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl.hpp
index 3db844920b..cd92d1f246 100644
--- a/src/modules/mc_pos_control/PositionControl.hpp
+++ b/src/modules/mc_pos_control/PositionControl.hpp
@@ -111,54 +111,54 @@ public:
* @see _yawspeed_sp
* @param dt the delta-time
*/
- void generateThrustYawSetpoint(const float &dt);
+ void generateThrustYawSetpoint(const float dt);
/**
* Set the integral term in xy to 0.
* @see _thr_int
*/
- void resetIntegralXY() {_thr_int(0) = _thr_int(1) = 0.0f;};
+ void resetIntegralXY() { _thr_int(0) = _thr_int(1) = 0.0f; }
/**
* Set the integral term in z to 0.
* @see _thr_int
*/
- void resetIntegralZ() {_thr_int(2) = 0.0f;};
+ void resetIntegralZ() { _thr_int(2) = 0.0f; }
/**
* Get the
* @see _thr_sp
* @return The thrust set-point member.
*/
- matrix::Vector3f getThrustSetpoint() {return _thr_sp;}
+ const matrix::Vector3f &getThrustSetpoint() { return _thr_sp; }
/**
* Get the
* @see _yaw_sp
* @return The yaw set-point member.
*/
- float getYawSetpoint() { return _yaw_sp;}
+ const float &getYawSetpoint() { return _yaw_sp; }
/**
* Get the
* @see _yawspeed_sp
* @return The yawspeed set-point member.
*/
- float getYawspeedSetpoint() {return _yawspeed_sp;}
+ const float &getYawspeedSetpoint() { return _yawspeed_sp; }
/**
* Get the
* @see _vel_sp
* @return The velocity set-point member.
*/
- matrix::Vector3f getVelSp() {return _vel_sp;}
+ const matrix::Vector3f &getVelSp() { return _vel_sp; }
/**
* Get the
* @see _pos_sp
* @return The position set-point member.
*/
- matrix::Vector3f getPosSp() {return _pos_sp;}
+ const matrix::Vector3f &getPosSp() { return _pos_sp; }
protected:
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 7d10d85747..4535c46caf 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -122,15 +122,14 @@ private:
float _takeoff_speed = -1.f; /**< For flighttask interface used only. It can be thrust or velocity setpoints */
float _takeoff_reference_z; /**< Z-position when takeoff was initiated */
- vehicle_status_s _vehicle_status{}; /**< vehicle status */
- vehicle_land_detected_s _vehicle_land_detected{}; /**< vehicle land detected */
- vehicle_attitude_setpoint_s _att_sp{}; /**< vehicle attitude setpoint */
+ vehicle_status_s _vehicle_status{}; /**< vehicle status */
+ vehicle_land_detected_s _vehicle_land_detected{}; /**< vehicle land detected */
+ vehicle_attitude_setpoint_s _att_sp{}; /**< vehicle attitude setpoint */
vehicle_control_mode_s _control_mode{}; /**< vehicle control mode */
- vehicle_local_position_s _local_pos{}; /**< vehicle local position */
- vehicle_local_position_setpoint_s _local_pos_sp{}; /**< vehicle local position setpoint */
- home_position_s _home_pos{}; /**< home position */
- vehicle_trajectory_waypoint_s _traj_wp_avoidance{}; /**< trajectory waypoint */
- vehicle_trajectory_waypoint_s _traj_wp_avoidance_desired{}; /**< desired waypoints, inputs to an obstacle avoidance module */
+ vehicle_local_position_s _local_pos{}; /**< vehicle local position */
+ home_position_s _home_pos{}; /**< home position */
+ vehicle_trajectory_waypoint_s _traj_wp_avoidance{}; /**< trajectory waypoint */
+ vehicle_trajectory_waypoint_s _traj_wp_avoidance_desired{}; /**< desired waypoints, inputs to an obstacle avoidance module */
DEFINE_PARAMETERS(
(ParamFloat<px4::params::MPC_TKO_RAMP_T>) _takeoff_ramp_time, /**< time constant for smooth takeoff ramp */
@@ -212,7 +211,7 @@ private:
* Publish local position setpoint.
* This is only required for logging.
*/
- void publish_local_pos_sp();
+ void publish_local_pos_sp(const vehicle_local_position_setpoint_s &local_pos_sp);
/**
* Checks if smooth takeoff is initiated.
@@ -698,17 +697,22 @@ MulticopterPositionControl::task_main()
}
// Fill local position, velocity and thrust setpoint.
- _local_pos_sp.timestamp = hrt_absolute_time();
- _local_pos_sp.x = _control.getPosSp()(0);
- _local_pos_sp.y = _control.getPosSp()(1);
- _local_pos_sp.z = _control.getPosSp()(2);
- _local_pos_sp.yaw = _control.getYawSetpoint();
- _local_pos_sp.yawspeed = _control.getYawspeedSetpoint();
-
- _local_pos_sp.vx = _control.getVelSp()(0);
- _local_pos_sp.vy = _control.getVelSp()(1);
- _local_pos_sp.vz = _control.getVelSp()(2);
- thr_sp.copyTo(_local_pos_sp.thrust);
+ vehicle_local_position_setpoint_s local_pos_sp{};
+ local_pos_sp.timestamp = hrt_absolute_time();
+ local_pos_sp.x = _control.getPosSp()(0);
+ local_pos_sp.y = _control.getPosSp()(1);
+ local_pos_sp.z = _control.getPosSp()(2);
+ local_pos_sp.yaw = _control.getYawSetpoint();
+ local_pos_sp.yawspeed = _control.getYawspeedSetpoint();
+
+ local_pos_sp.vx = _control.getVelSp()(0);
+ local_pos_sp.vy = _control.getVelSp()(1);
+ local_pos_sp.vz = _control.getVelSp()(2);
+ thr_sp.copyTo(local_pos_sp.thrust);
+
+ // Publish local position setpoint (for logging only) and attitude setpoint (for attitude controller).
+ publish_local_pos_sp(local_pos_sp);
+
// Fill attitude setpoint. Attitude is computed from yaw and thrust setpoint.
_att_sp = ControlMath::thrustToAttitude(thr_sp, _control.getYawSetpoint());
@@ -727,9 +731,6 @@ MulticopterPositionControl::task_main()
}
}
- // Publish local position setpoint (for logging only) and attitude setpoint (for attitude controller).
- publish_local_pos_sp();
-
// publish attitude setpoint
// Note: this requires review. The reason for not sending
// an attitude setpoint is because for non-flighttask modes
@@ -1084,19 +1085,14 @@ MulticopterPositionControl::publish_attitude()
}
void
-MulticopterPositionControl::publish_local_pos_sp()
+MulticopterPositionControl::publish_local_pos_sp(const vehicle_local_position_setpoint_s &local_pos_sp)
{
- _local_pos_sp.timestamp = hrt_absolute_time();
-
// publish local position setpoint
if (_local_pos_sp_pub != nullptr) {
- orb_publish(ORB_ID(vehicle_local_position_setpoint),
- _local_pos_sp_pub, &_local_pos_sp);
+ orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &local_pos_sp);
} else {
- _local_pos_sp_pub = orb_advertise(
- ORB_ID(vehicle_local_position_setpoint),
- &_local_pos_sp);
+ _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp);
}
}
--
GitLab