fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-17 19:47:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

offboard setpoints: correctly check for force setpoint

Browse Source
This commit is contained in:
Thomas Gubler
2014-08-12 14:54:35 +02:00
parent 8bbaacb1e9
commit a04d70ed5a
2 changed files with 35 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+5 -1
View File
@@ -441,7 +441,11 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
}
if (_control_mode.flag_control_offboard_enabled) {
if (offboard_control_sp.isForceSetpoint) {
if (offboard_control_sp.isForceSetpoint &&
offboard_control_sp_ignore_position_all(offboard_control_sp) &&
offboard_control_sp_ignore_velocity_all(offboard_control_sp)) {
/* The offboard setpoint is a force setpoint only, directly writing to the force
* setpoint topic and not publishing the setpoint triplet topic */
struct vehicle_force_setpoint_s force_sp;
force_sp.x = offboard_control_sp.acceleration[0];
force_sp.y = offboard_control_sp.acceleration[1];
src/modules/uORB/topics/offboard_control_setpoint.h
+30
View File
@@ -104,14 +104,44 @@ struct offboard_control_setpoint_s {
* @}
*/
/**
* Returns true if the position setpoint at index should be ignored
*/
inline bool offboard_control_sp_ignore_position(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
return (bool)(offboard_control_sp.ignore & (1 << index));
}
/**
* Returns true if all position setpoints should be ignored
*/
inline bool offboard_control_sp_ignore_position_all(const struct offboard_control_setpoint_s &offboard_control_sp) {
for (int i = 0; i < 3; i++) {
if (offboard_control_sp_ignore_position(offboard_control_sp, i)) {
return true;
}
}
return false;
}
/**
* Returns true if the velocity setpoint at index should be ignored
*/
inline bool offboard_control_sp_ignore_velocity(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
return (bool)(offboard_control_sp.ignore & (1 << (3 + index)));
}
/**
* Returns true if all velocity setpoints should be ignored
*/
inline bool offboard_control_sp_ignore_velocity_all(const struct offboard_control_setpoint_s &offboard_control_sp) {
for (int i = 0; i < 3; i++) {
if (offboard_control_sp_ignore_velocity(offboard_control_sp, i)) {
return true;
}
}
return false;
}
inline bool offboard_control_sp_ignore_acceleration(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
return (bool)(offboard_control_sp.ignore & (1 << (6 + index)));
}