fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-01 07:51:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #1857 from PX4/mcposoffboardnoattitudepub

Browse Source 
mc pos ctrl (multiplatform): do not publish att sp in offboard && no position/velocity control
This commit is contained in:
Lorenz Meier
2015-03-02 08:27:23 +01:00
parent dc67834222 0dec33526b
commit 2aa91a05fe
2 changed files with 28 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+16 -9
View File
@@ -761,8 +761,8 @@ void MulticopterPositionControl::control_auto(float dt)
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
//Make sure that the position setpoint is valid //Make sure that the position setpoint is valid
if (!isfinite(_pos_sp_triplet.current.lat) || if (!isfinite(_pos_sp_triplet.current.lat) ||
!isfinite(_pos_sp_triplet.current.lon) || !isfinite(_pos_sp_triplet.current.lon) ||
!isfinite(_pos_sp_triplet.current.alt)) { !isfinite(_pos_sp_triplet.current.alt)) {
_pos_sp_triplet.current.valid = false; _pos_sp_triplet.current.valid = false;
} }
@@ -1367,7 +1367,7 @@ MulticopterPositionControl::task_main()
} else if (yaw_offs > YAW_OFFSET_MAX) { } else if (yaw_offs > YAW_OFFSET_MAX) {
_att_sp.yaw_body = _wrap_pi(_att.yaw + YAW_OFFSET_MAX); _att_sp.yaw_body = _wrap_pi(_att.yaw + YAW_OFFSET_MAX);
} }
} }
//Control roll and pitch directly if we no aiding velocity controller is active //Control roll and pitch directly if we no aiding velocity controller is active
@@ -1388,15 +1388,22 @@ MulticopterPositionControl::task_main()
_att_sp.timestamp = hrt_absolute_time(); _att_sp.timestamp = hrt_absolute_time();
} }
else { else {
reset_yaw_sp = true; reset_yaw_sp = true;
} }
// publish attitude setpoint /* publish attitude setpoint
if (_att_sp_pub > 0) { * Do not publish if offboard is enabled but position/velocity control is disabled,
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); * in this case the attitude setpoint is published by the mavlink app
*/
if (!(_control_mode.flag_control_offboard_enabled &&
!(_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_velocity_enabled))) {
if (_att_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
} else { } else {
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
}
} }
/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
+12 -5
View File
@@ -1015,12 +1015,19 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti
reset_yaw_sp = true; reset_yaw_sp = true;
} }
/* publish attitude setpoint */ /* publish attitude setpoint
if (_att_sp_pub != nullptr) { * Do not publish if offboard is enabled but position/velocity control is disabled, in this case the attitude setpoint
_att_sp_pub->publish(_att_sp_msg); * is published by the mavlink app
*/
if (!(_control_mode->data().flag_control_offboard_enabled &&
!(_control_mode->data().flag_control_position_enabled ||
_control_mode->data().flag_control_velocity_enabled))) {
if (_att_sp_pub != nullptr) {
_att_sp_pub->publish(_att_sp_msg);
} else { } else {
_att_sp_pub = _n.advertise<px4_vehicle_attitude_setpoint>(); _att_sp_pub = _n.advertise<px4_vehicle_attitude_setpoint>();
}
} }
/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */