mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-21 21:43:33 +08:00
introduce offboard control mode topic
Replace offboard_control_setpoint with offboard_control_mode Remove all setpoint data from the topic as it's not used anymore (setpoint data is directly routed into position/attitude setpoint topics for some time now) Remove mode enum and replace with ignore booleans which map better to the mavlink message Mavlink: Rework parsing of offboard setpoints Commander: in offboard mode set control flags based on ignore flags instead of enum
This commit is contained in:
Thomas Gubler
@@ -108,7 +108,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
|
||||
_cmd_pub(-1),
|
||||
_flow_pub(-1),
|
||||
_range_pub(-1),
|
||||
_offboard_control_sp_pub(-1),
|
||||
_offboard_control_mode_pub(-1),
|
||||
_global_vel_sp_pub(-1),
|
||||
_att_sp_pub(-1),
|
||||
_rates_sp_pub(-1),
|
||||
@@ -517,8 +517,8 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
|
||||
mavlink_set_position_target_local_ned_t set_position_target_local_ned;
|
||||
mavlink_msg_set_position_target_local_ned_decode(msg, &set_position_target_local_ned);
|
||||
|
||||
struct offboard_control_setpoint_s offboard_control_sp;
|
||||
memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));//XXX breaks compatibility with multiple setpoints
|
||||
struct offboard_control_mode_s offboard_control_mode;
|
||||
memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));//XXX breaks compatibility with multiple setpoints
|
||||
|
||||
/* Only accept messages which are intended for this system */
|
||||
if ((mavlink_system.sysid == set_position_target_local_ned.target_system ||
|
||||
@@ -527,64 +527,24 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
|
||||
set_position_target_local_ned.target_component == 0)) {
|
||||
|
||||
/* convert mavlink type (local, NED) to uORB offboard control struct */
|
||||
switch (set_position_target_local_ned.coordinate_frame) {
|
||||
case MAV_FRAME_LOCAL_NED:
|
||||
offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED;
|
||||
break;
|
||||
case MAV_FRAME_LOCAL_OFFSET_NED:
|
||||
offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED;
|
||||
break;
|
||||
case MAV_FRAME_BODY_NED:
|
||||
offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED;
|
||||
break;
|
||||
case MAV_FRAME_BODY_OFFSET_NED:
|
||||
offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED;
|
||||
break;
|
||||
default:
|
||||
/* invalid setpoint, avoid publishing */
|
||||
return;
|
||||
}
|
||||
offboard_control_sp.position[0] = set_position_target_local_ned.x;
|
||||
offboard_control_sp.position[1] = set_position_target_local_ned.y;
|
||||
offboard_control_sp.position[2] = set_position_target_local_ned.z;
|
||||
offboard_control_sp.velocity[0] = set_position_target_local_ned.vx;
|
||||
offboard_control_sp.velocity[1] = set_position_target_local_ned.vy;
|
||||
offboard_control_sp.velocity[2] = set_position_target_local_ned.vz;
|
||||
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));
|
||||
offboard_control_mode.ignore_position = (bool)(set_position_target_local_ned.type_mask & 0x7);
|
||||
offboard_control_mode.ignore_velocity = (bool)(set_position_target_local_ned.type_mask & 0x38);
|
||||
offboard_control_mode.ignore_acceleration_force = (bool)(set_position_target_local_ned.type_mask & 0x1C0);
|
||||
bool is_force_sp = (bool)(set_position_target_local_ned.type_mask & (1 << 9));
|
||||
/* yaw ignore flag mapps to ignore_attitude */
|
||||
offboard_control_mode.ignore_attitude = (bool)(set_position_target_local_ned.type_mask & 0x400);
|
||||
/* yawrate ignore flag mapps to ignore_bodyrate */
|
||||
offboard_control_mode.ignore_bodyrate = (bool)(set_position_target_local_ned.type_mask & 0x800);
|
||||
|
||||
/* If we are in force control mode, for now set offboard mode to force control */
|
||||
if (offboard_control_sp.isForceSetpoint) {
|
||||
offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_FORCE;
|
||||
}
|
||||
|
||||
/* set ignore flags */
|
||||
for (int i = 0; i < 9; i++) {
|
||||
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);
|
||||
if (set_position_target_local_ned.type_mask & (1 << 10)) {
|
||||
offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAW);
|
||||
}
|
||||
offboard_control_mode.timestamp = hrt_absolute_time();
|
||||
|
||||
offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAWRATE);
|
||||
if (set_position_target_local_ned.type_mask & (1 << 11)) {
|
||||
offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAWRATE);
|
||||
}
|
||||
|
||||
offboard_control_sp.timestamp = hrt_absolute_time();
|
||||
|
||||
if (_offboard_control_sp_pub < 0) {
|
||||
_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
|
||||
if (_offboard_control_mode_pub < 0) {
|
||||
_offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp);
|
||||
orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode);
|
||||
}
|
||||
|
||||
/* If we are in offboard control mode and offboard control loop through is enabled
|
||||
@@ -596,15 +556,14 @@ 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 &&
|
||||
offboard_control_sp_ignore_position_all(offboard_control_sp) &&
|
||||
offboard_control_sp_ignore_velocity_all(offboard_control_sp)) {
|
||||
if (is_force_sp && offboard_control_mode.ignore_position &&
|
||||
offboard_control_mode.ignore_velocity) {
|
||||
/* 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];
|
||||
force_sp.z = offboard_control_sp.acceleration[2];
|
||||
force_sp.x = set_position_target_local_ned.afx;
|
||||
force_sp.y = set_position_target_local_ned.afy;
|
||||
force_sp.z = set_position_target_local_ned.afz;
|
||||
//XXX: yaw
|
||||
if (_force_sp_pub < 0) {
|
||||
_force_sp_pub = orb_advertise(ORB_ID(vehicle_force_setpoint), &force_sp);
|
||||
@@ -619,62 +578,53 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
|
||||
pos_sp_triplet.current.valid = true;
|
||||
pos_sp_triplet.current.type = position_setpoint_s::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)) {
|
||||
/* set the local pos values */
|
||||
if (!offboard_control_mode.ignore_position) {
|
||||
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];
|
||||
pos_sp_triplet.current.z = offboard_control_sp.position[2];
|
||||
pos_sp_triplet.current.x = set_position_target_local_ned.x;
|
||||
pos_sp_triplet.current.y = set_position_target_local_ned.y;
|
||||
pos_sp_triplet.current.z = set_position_target_local_ned.z;
|
||||
} else {
|
||||
pos_sp_triplet.current.position_valid = false;
|
||||
}
|
||||
|
||||
/* 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_some(offboard_control_sp)) {
|
||||
/* set the local vel values */
|
||||
if (!offboard_control_mode.ignore_velocity) {
|
||||
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];
|
||||
pos_sp_triplet.current.vz = offboard_control_sp.velocity[2];
|
||||
pos_sp_triplet.current.vx = set_position_target_local_ned.vx;
|
||||
pos_sp_triplet.current.vy = set_position_target_local_ned.vy;
|
||||
pos_sp_triplet.current.vz = set_position_target_local_ned.vz;
|
||||
} else {
|
||||
pos_sp_triplet.current.velocity_valid = false;
|
||||
}
|
||||
|
||||
/* 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_some(offboard_control_sp)) {
|
||||
if (!offboard_control_mode.ignore_acceleration_force) {
|
||||
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];
|
||||
pos_sp_triplet.current.a_z = offboard_control_sp.acceleration[2];
|
||||
pos_sp_triplet.current.a_x = set_position_target_local_ned.afx;
|
||||
pos_sp_triplet.current.a_y = set_position_target_local_ned.afy;
|
||||
pos_sp_triplet.current.a_z = set_position_target_local_ned.afz;
|
||||
pos_sp_triplet.current.acceleration_is_force =
|
||||
offboard_control_sp.isForceSetpoint;
|
||||
is_force_sp;
|
||||
|
||||
} 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)) {
|
||||
/* set the yaw sp value */
|
||||
if (!offboard_control_mode.ignore_attitude) {
|
||||
pos_sp_triplet.current.yaw_valid = true;
|
||||
pos_sp_triplet.current.yaw = offboard_control_sp.yaw;
|
||||
pos_sp_triplet.current.yaw = set_position_target_local_ned.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)) {
|
||||
/* set the yawrate sp value */
|
||||
if (!offboard_control_mode.ignore_bodyrate) {
|
||||
pos_sp_triplet.current.yawspeed_valid = true;
|
||||
pos_sp_triplet.current.yawspeed = offboard_control_sp.yaw_rate;
|
||||
pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate;
|
||||
|
||||
} else {
|
||||
pos_sp_triplet.current.yawspeed_valid = false;
|
||||
@@ -743,42 +693,29 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
|
||||
mavlink_set_attitude_target_t set_attitude_target;
|
||||
mavlink_msg_set_attitude_target_decode(msg, &set_attitude_target);
|
||||
|
||||
struct offboard_control_setpoint_s offboard_control_sp;
|
||||
memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); //XXX breaks compatibility with multiple setpoints
|
||||
struct offboard_control_mode_s offboard_control_mode;
|
||||
memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));
|
||||
|
||||
/* Only accept messages which are intended for this system */
|
||||
if ((mavlink_system.sysid == set_attitude_target.target_system ||
|
||||
set_attitude_target.target_system == 0) &&
|
||||
(mavlink_system.compid == set_attitude_target.target_component ||
|
||||
set_attitude_target.target_component == 0)) {
|
||||
for (int i = 0; i < 4; i++) {
|
||||
offboard_control_sp.attitude[i] = set_attitude_target.q[i];
|
||||
}
|
||||
offboard_control_sp.attitude_rate[0] = set_attitude_target.body_roll_rate;
|
||||
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 + OFB_IGN_BIT_BODYRATE_X));
|
||||
offboard_control_sp.ignore |= (set_attitude_target.type_mask & (1 << i)) << OFB_IGN_BIT_BODYRATE_X;
|
||||
}
|
||||
offboard_control_mode.ignore_bodyrate = (bool)(set_attitude_target.type_mask & 0x7);
|
||||
/* 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);
|
||||
offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6));
|
||||
/* 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_mode.ignore_attitude = (bool)(set_attitude_target.type_mask & (1 << 7));
|
||||
|
||||
offboard_control_mode.timestamp = hrt_absolute_time();
|
||||
|
||||
offboard_control_sp.timestamp = hrt_absolute_time();
|
||||
offboard_control_sp.mode =OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; //XXX handle rate control mode
|
||||
|
||||
if (_offboard_control_sp_pub < 0) {
|
||||
_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
|
||||
if (_offboard_control_mode_pub < 0) {
|
||||
_offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp);
|
||||
orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode);
|
||||
}
|
||||
|
||||
/* If we are in offboard control mode and offboard control loop through is enabled
|
||||
@@ -793,8 +730,8 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
|
||||
if (_control_mode.flag_control_offboard_enabled) {
|
||||
|
||||
/* 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))) {
|
||||
if (!(offboard_control_mode.ignore_attitude ||
|
||||
offboard_control_mode.ignore_thrust)) {
|
||||
struct vehicle_attitude_setpoint_s att_sp;
|
||||
att_sp.timestamp = hrt_absolute_time();
|
||||
mavlink_quaternion_to_euler(set_attitude_target.q,
|
||||
@@ -814,8 +751,8 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
|
||||
|
||||
/* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */
|
||||
///XXX add support for ignoring individual axes
|
||||
if (!(offboard_control_sp_ignore_bodyrates_some(offboard_control_sp) ||
|
||||
offboard_control_sp_ignore_thrust(offboard_control_sp))) {
|
||||
if (!(offboard_control_mode.ignore_bodyrate ||
|
||||
offboard_control_mode.ignore_thrust)) {
|
||||
struct vehicle_rates_setpoint_s rates_sp;
|
||||
rates_sp.timestamp = hrt_absolute_time();
|
||||
rates_sp.roll = set_attitude_target.body_roll_rate;
|
||||
|
||||
Reference in New Issue
Block a user