fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-14 07:10:34 +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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+26 -30
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);
}
}