fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

multicopter position controller use const references

Browse Source
This commit is contained in:
Daniel Agar 2018-08-28 14:57:47 -04:00
parent e26bfd2e79
commit 223dacee64
6 changed files with 48 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/lib/FlightTasks/FlightTasks.cpp
View File

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

2
src/lib/FlightTasks/FlightTasks.hpp
View File

@ -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)

6
src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp
View File

@ -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.

18
src/modules/mc_pos_control/PositionControl.cpp
View File

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

16
src/modules/mc_pos_control/PositionControl.hpp
View File

@ -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:

56
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -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();
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);
_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);
// 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);
}
}