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

offboard: ignore mask cleanup

Browse Source 
add handling of thrust ignore bit, add defines for the bitnumbers,
improve and fix interface to ignore bitmask
This commit is contained in:
Thomas Gubler
2014-08-15 12:58:06 +02:00
parent 874846969c
commit 1e9d77f1d2
2 changed files with 100 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+18 -11
View File
@@ -472,7 +472,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
/* set the local pos values if the setpoint type is 'local pos' and none
* of the local pos fields is set to 'ignore' */
if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
!offboard_control_sp_ignore_position_all(offboard_control_sp)) {
!offboard_control_sp_ignore_position_some(offboard_control_sp)) {
pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others
pos_sp_triplet.current.position_valid = true;
pos_sp_triplet.current.x = offboard_control_sp.position[0];
@@ -485,7 +485,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
/* set the local vel values if the setpoint type is 'local pos' and none
* of the local vel fields is set to 'ignore' */
if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
!offboard_control_sp_ignore_velocity_all(offboard_control_sp)) {
!offboard_control_sp_ignore_velocity_some(offboard_control_sp)) {
pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others
pos_sp_triplet.current.velocity_valid = true;
pos_sp_triplet.current.vx = offboard_control_sp.velocity[0];
@@ -498,7 +498,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
/* set the local acceleration values if the setpoint type is 'local pos' and none
* of the accelerations fields is set to 'ignore' */
if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
!offboard_control_sp_ignore_acceleration_all(offboard_control_sp)) {
!offboard_control_sp_ignore_acceleration_some(offboard_control_sp)) {
pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others
pos_sp_triplet.current.acceleration_valid = true;
pos_sp_triplet.current.a_x = offboard_control_sp.acceleration[0];
@@ -589,12 +589,17 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
offboard_control_sp.attitude_rate[1] = set_attitude_target.body_pitch_rate;
offboard_control_sp.attitude_rate[2] = set_attitude_target.body_yaw_rate;
/* set correct ignore flags for body rate fields: copy from mavlink message */
for (int i = 0; i < 3; i++) {
offboard_control_sp.ignore &= ~(1 << (i + 9));
offboard_control_sp.ignore |= (set_attitude_target.type_mask & (1 << i)) << 9;
offboard_control_sp.ignore &= ~(1 << (i + OFB_IGN_BIT_BODYRATE_X));
offboard_control_sp.ignore |= (set_attitude_target.type_mask & (1 << i)) << OFB_IGN_BIT_BODYRATE_X;
}
offboard_control_sp.ignore &= ~(1 << 10);
offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 7)) << 3);
/* set correct ignore flags for thrust field: copy from mavlink message */
offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_THRUST);
offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 6)) << OFB_IGN_BIT_THRUST);
/* set correct ignore flags for attitude field: copy from mavlink message */
offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_ATT);
offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 7)) << OFB_IGN_BIT_ATT);
offboard_control_sp.timestamp = hrt_absolute_time();
@@ -618,8 +623,9 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
if (_control_mode.flag_control_offboard_enabled) {
/* Publish attitude setpoint if ignore bit is not set */
if (!(set_attitude_target.type_mask & (1 << 7))) {
/* Publish attitude setpoint if attitude and thrust ignore bits are not set */
if (!(offboard_control_sp_ignore_attitude(offboard_control_sp) ||
offboard_control_sp_ignore_thrust(offboard_control_sp))) {
struct vehicle_attitude_setpoint_s att_sp;
att_sp.timestamp = hrt_absolute_time();
mavlink_quaternion_to_euler(set_attitude_target.q,
@@ -634,9 +640,10 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
}
}
/* Publish attitude rate setpoint if ignore bit are not set */
/* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */
///XXX add support for ignoring individual axes
if (!(set_attitude_target.type_mask & (0b111))) {
if (!(offboard_control_sp_ignore_bodyrates_some(offboard_control_sp) ||
offboard_control_sp_ignore_thrust(offboard_control_sp))) {
struct vehicle_rates_setpoint_s rates_sp;
rates_sp.timestamp = hrt_absolute_time();
rates_sp.roll = set_attitude_target.body_roll_rate;
src/modules/uORB/topics/offboard_control_setpoint.h
+82 -8
View File
@@ -61,7 +61,7 @@ enum OFFBOARD_CONTROL_MODE {
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 */
OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 11 /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */
};
enum OFFBOARD_CONTROL_FRAME {
@@ -72,6 +72,23 @@ enum OFFBOARD_CONTROL_FRAME {
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
};
/**
* @addtogroup topics
* @{
@@ -87,9 +104,11 @@ struct offboard_control_setpoint_s {
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 */
uint16_t ignore; /**< if field i is set to true, the value should be ignored, see definition at top of file
for mapping */
uint16_t ignore; /**< if field i is set to true, pi should be ignored */
//XXX define constants for bit offsets
bool isForceSetpoint; /**< the acceleration vector should be interpreted as force */
float override_mode_switch;
@@ -108,13 +127,25 @@ 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));
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;
@@ -127,13 +158,25 @@ inline bool offboard_control_sp_ignore_position_all(const struct offboard_contro
* 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)));
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;
@@ -146,13 +189,25 @@ inline bool offboard_control_sp_ignore_velocity_all(const struct offboard_contro
* 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 << (6 + 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;
@@ -165,14 +220,33 @@ inline bool offboard_control_sp_ignore_acceleration_all(const struct offboard_co
* 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 << (9 + 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 << 10));
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));
}