diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-08-12 14:54:35 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-08-12 14:57:14 +0200 |
commit | a04d70ed5a800ff6d74afc6800f7bb94ed472ad4 (patch) | |
tree | e208e473c9f57b2573d2ceec71ec9cb98054039d /src/modules | |
parent | 8bbaacb1e9381c29a83e0ecf37de6df3018bd38d (diff) | |
download | px4-firmware-a04d70ed5a800ff6d74afc6800f7bb94ed472ad4.tar.gz px4-firmware-a04d70ed5a800ff6d74afc6800f7bb94ed472ad4.tar.bz2 px4-firmware-a04d70ed5a800ff6d74afc6800f7bb94ed472ad4.zip |
offboard setpoints: correctly check for force setpoint
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 6 | ||||
-rw-r--r-- | src/modules/uORB/topics/offboard_control_setpoint.h | 30 |
2 files changed, 35 insertions, 1 deletions
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index dfc5ddc91..418b81ee1 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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]; diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 19f11ba92..4f45ac14f 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -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))); } |