multicopter position controller use const references

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;
 }

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)

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.

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 &&

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:
 

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);
 	}
 }