diff options
Diffstat (limited to 'src/modules/uORB/topics/offboard_control_setpoint.h')
| -rw-r--r-- | src/modules/uORB/topics/offboard_control_setpoint.h | 201 |
1 files changed, 192 insertions, 9 deletions
diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index d7b131e3c..72a28e501 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -53,11 +53,42 @@ enum OFFBOARD_CONTROL_MODE { OFFBOARD_CONTROL_MODE_DIRECT = 0, OFFBOARD_CONTROL_MODE_DIRECT_RATES = 1, OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE = 2, - OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY = 3, - OFFBOARD_CONTROL_MODE_DIRECT_POSITION = 4, - OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 5, - OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 6, - OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 7, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ + OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED = 3, + OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED = 4, + OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED = 5, + OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED = 6, + OFFBOARD_CONTROL_MODE_DIRECT_GLOBAL = 7, + OFFBOARD_CONTROL_MODE_DIRECT_FORCE = 8, + OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 9, + OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 10, + OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 11 /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ +}; + +enum OFFBOARD_CONTROL_FRAME { + OFFBOARD_CONTROL_FRAME_LOCAL_NED = 0, + OFFBOARD_CONTROL_FRAME_LOCAL_OFFSET_NED = 1, + OFFBOARD_CONTROL_FRAME_BODY_NED = 2, + OFFBOARD_CONTROL_FRAME_BODY_OFFSET_NED = 3, + OFFBOARD_CONTROL_FRAME_GLOBAL = 4 +}; + +/* mappings for the ignore bitmask */ +enum {OFB_IGN_BIT_POS_X, + OFB_IGN_BIT_POS_Y, + OFB_IGN_BIT_POS_Z, + OFB_IGN_BIT_VEL_X, + OFB_IGN_BIT_VEL_Y, + OFB_IGN_BIT_VEL_Z, + OFB_IGN_BIT_ACC_X, + OFB_IGN_BIT_ACC_Y, + OFB_IGN_BIT_ACC_Z, + OFB_IGN_BIT_BODYRATE_X, + OFB_IGN_BIT_BODYRATE_Y, + OFB_IGN_BIT_BODYRATE_Z, + OFB_IGN_BIT_ATT, + OFB_IGN_BIT_THRUST, + OFB_IGN_BIT_YAW, + OFB_IGN_BIT_YAWRATE, }; /** @@ -70,10 +101,21 @@ struct offboard_control_setpoint_s { enum OFFBOARD_CONTROL_MODE mode; /**< The current control inputs mode */ - float p1; /**< ailerons roll / roll rate input */ - float p2; /**< elevator / pitch / pitch rate */ - float p3; /**< rudder / yaw rate / yaw */ - float p4; /**< throttle / collective thrust / altitude */ + double position[3]; /**< lat, lon, alt / x, y, z */ + float velocity[3]; /**< x vel, y vel, z vel */ + float acceleration[3]; /**< x acc, y acc, z acc */ + float attitude[4]; /**< attitude of vehicle (quaternion) */ + float attitude_rate[3]; /**< body angular rates (x, y, z) */ + float thrust; /**< thrust */ + float yaw; /**< yaw: this is the yaw from the position_target message + (not from the full attitude_target message) */ + float yaw_rate; /**< yaw rate: this is the yaw from the position_target message + (not from the full attitude_target message) */ + + uint16_t ignore; /**< if field i is set to true, the value should be ignored, see definition at top of file + for mapping */ + + bool isForceSetpoint; /**< the acceleration vector should be interpreted as force */ float override_mode_switch; @@ -87,6 +129,147 @@ 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 << (OFB_IGN_BIT_POS_X + 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 false; + } + } + return true; +} + +/** + * Returns true if some position setpoints should be ignored + */ +inline bool offboard_control_sp_ignore_position_some(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 << (OFB_IGN_BIT_VEL_X + 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 false; + } + } + return true; +} + +/** + * Returns true if some velocity setpoints should be ignored + */ +inline bool offboard_control_sp_ignore_velocity_some(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; +} + +/** + * Returns true if the acceleration setpoint at index should be ignored + */ +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 << (OFB_IGN_BIT_ACC_X + index))); +} + +/** + * Returns true if all acceleration setpoints should be ignored + */ +inline bool offboard_control_sp_ignore_acceleration_all(const struct offboard_control_setpoint_s &offboard_control_sp) { + for (int i = 0; i < 3; i++) { + if (!offboard_control_sp_ignore_acceleration(offboard_control_sp, i)) { + return false; + } + } + return true; +} + +/** + * Returns true if some acceleration setpoints should be ignored + */ +inline bool offboard_control_sp_ignore_acceleration_some(const struct offboard_control_setpoint_s &offboard_control_sp) { + for (int i = 0; i < 3; i++) { + if (offboard_control_sp_ignore_acceleration(offboard_control_sp, i)) { + return true; + } + } + return false; +} + +/** + * Returns true if the bodyrate setpoint at index should be ignored + */ +inline bool offboard_control_sp_ignore_bodyrates(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { + return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_BODYRATE_X + index))); +} + +/** + * Returns true if some of the bodyrate setpoints should be ignored + */ +inline bool offboard_control_sp_ignore_bodyrates_some(const struct offboard_control_setpoint_s &offboard_control_sp) { + for (int i = 0; i < 3; i++) { + if (offboard_control_sp_ignore_bodyrates(offboard_control_sp, i)) { + return true; + } + } + return false; +} + +/** + * Returns true if the attitude setpoint should be ignored + */ +inline bool offboard_control_sp_ignore_attitude(const struct offboard_control_setpoint_s &offboard_control_sp) { + return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_ATT)); +} + +/** + * Returns true if the thrust setpoint should be ignored + */ +inline bool offboard_control_sp_ignore_thrust(const struct offboard_control_setpoint_s &offboard_control_sp) { + return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_THRUST)); +} + +/** + * Returns true if the yaw setpoint should be ignored + */ +inline bool offboard_control_sp_ignore_yaw(const struct offboard_control_setpoint_s &offboard_control_sp) { + return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_YAW)); +} + +/** + * Returns true if the yaw rate setpoint should be ignored + */ +inline bool offboard_control_sp_ignore_yawrate(const struct offboard_control_setpoint_s &offboard_control_sp) { + return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_YAWRATE)); +} + + /* register this as object request broker structure */ ORB_DECLARE(offboard_control_setpoint); |