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