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

support new yaw and yawrate fields in mavlnk position_target message

Browse Source
This commit is contained in:
Thomas Gubler
2014-08-25 09:19:36 +02:00
parent 9be755ec61
commit 55fde23233
3 changed files with 54 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+31 -4
View File
@@ -474,6 +474,8 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
offboard_control_sp.acceleration[0] = set_position_target_local_ned.afx;
offboard_control_sp.acceleration[1] = set_position_target_local_ned.afy;
offboard_control_sp.acceleration[2] = set_position_target_local_ned.afz;
offboard_control_sp.yaw = set_position_target_local_ned.yaw;
offboard_control_sp.yaw_rate = set_position_target_local_ned.yaw_rate;
offboard_control_sp.isForceSetpoint = (bool)(set_position_target_local_ned.type_mask & (1 << 9));
/* If we are in force control mode, for now set offboard mode to force control */
@@ -486,7 +488,12 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
offboard_control_sp.ignore &= ~(1 << i);
offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << i));
}
offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAW);
offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 10)) <<
OFB_IGN_BIT_YAW;
offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAWRATE);
offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 11)) <<
OFB_IGN_BIT_YAWRATE;
offboard_control_sp.timestamp = hrt_absolute_time();
@@ -527,12 +534,12 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
pos_sp_triplet.previous.valid = false;
pos_sp_triplet.next.valid = false;
pos_sp_triplet.current.valid = true;
pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others
/* 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_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];
pos_sp_triplet.current.y = offboard_control_sp.position[1];
@@ -545,7 +552,6 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
* 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_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];
pos_sp_triplet.current.vy = offboard_control_sp.velocity[1];
@@ -558,7 +564,6 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
* of the accelerations fields is set to 'ignore' */
if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
!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];
pos_sp_triplet.current.a_y = offboard_control_sp.acceleration[1];
@@ -569,6 +574,28 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
} else {
pos_sp_triplet.current.acceleration_valid = false;
}
/* set the yaw sp value if the setpoint type is 'local pos' and the yaw
* field is not set to 'ignore' */
if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
!offboard_control_sp_ignore_yaw(offboard_control_sp)) {
pos_sp_triplet.current.yaw_valid = true;
pos_sp_triplet.current.yaw = offboard_control_sp.yaw;
} else {
pos_sp_triplet.current.yaw_valid = false;
}
/* set the yawrate sp value if the setpoint type is 'local pos' and the yawrate
* field is not set to 'ignore' */
if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
!offboard_control_sp_ignore_yawrate(offboard_control_sp)) {
pos_sp_triplet.current.yawspeed_valid = true;
pos_sp_triplet.current.yawspeed = offboard_control_sp.yaw_rate;
} else {
pos_sp_triplet.current.yawspeed_valid = false;
}
//XXX handle global pos setpoints (different MAV frames)
src/modules/uORB/topics/offboard_control_setpoint.h
+21 -1
View File
@@ -86,7 +86,9 @@ enum {OFB_IGN_BIT_POS_X,
OFB_IGN_BIT_BODYRATE_Y,
OFB_IGN_BIT_BODYRATE_Z,
OFB_IGN_BIT_ATT,
OFB_IGN_BIT_THRUST
OFB_IGN_BIT_THRUST,
OFB_IGN_BIT_YAW,
OFB_IGN_BIT_YAWRATE,
};
/**
@@ -105,6 +107,10 @@ struct offboard_control_setpoint_s {
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 */
@@ -249,6 +255,20 @@ inline bool offboard_control_sp_ignore_thrust(const struct offboard_control_setp
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);
src/modules/uORB/topics/position_setpoint_triplet.h
+2
View File
@@ -79,7 +79,9 @@ struct position_setpoint_s
double lon; /**< longitude, in deg */
float alt; /**< altitude AMSL, in m */
float yaw; /**< yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw */
bool yaw_valid; /**< true if yaw setpoint valid */
float yawspeed; /**< yawspeed (only for multirotors, in rad/s) */
bool yawspeed_valid; /**< true if yawspeed setpoint valid */
float loiter_radius; /**< loiter radius (only for fixed wing), in m */
int8_t loiter_direction; /**< loiter direction: 1 = CW, -1 = CCW */
float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */