diff options
author | Lorenz Meier <lorenz@px4.io> | 2015-02-28 17:47:23 +0100 |
---|---|---|
committer | Lorenz Meier <lorenz@px4.io> | 2015-02-28 17:47:23 +0100 |
commit | 1df0f25c6b112ef568f7a19a7819a8e7948d8515 (patch) | |
tree | 436b5a929e7e20b7bb33771b7eba6abe50865987 /src | |
parent | 19811bc73fb87662bd4e2a2145d3c4f5bc5aa201 (diff) | |
parent | 0f51907dd662ae6ebc9ab997dfeda273769cffce (diff) | |
download | px4-firmware-1df0f25c6b112ef568f7a19a7819a8e7948d8515.tar.gz px4-firmware-1df0f25c6b112ef568f7a19a7819a8e7948d8515.tar.bz2 px4-firmware-1df0f25c6b112ef568f7a19a7819a8e7948d8515.zip |
Merge pull request #1786 from PX4/offboardmode
Replace offboard control setpoint topic with simple offboard control mode topic, remove setpoint data from topic
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/commander/commander.cpp | 84 | ||||
-rw-r--r-- | src/modules/fw_att_control/fw_att_control_main.cpp | 35 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_messages.cpp | 1 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 277 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.h | 8 | ||||
-rw-r--r-- | src/modules/uORB/objects_common.cpp | 4 | ||||
-rw-r--r-- | src/modules/uORB/topics/offboard_control_mode.h | 73 | ||||
-rw-r--r-- | src/modules/uORB/topics/offboard_control_setpoint.h | 276 | ||||
-rw-r--r-- | src/modules/vtol_att_control/vtol_att_control_main.cpp | 52 |
9 files changed, 311 insertions, 499 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 242d8a486..2c5e0aa88 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -65,7 +65,7 @@ #include <uORB/topics/sensor_combined.h> #include <uORB/topics/battery_status.h> #include <uORB/topics/manual_control_setpoint.h> -#include <uORB/topics/offboard_control_setpoint.h> +#include <uORB/topics/offboard_control_mode.h> #include <uORB/topics/home_position.h> #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_local_position.h> @@ -180,7 +180,7 @@ static struct vehicle_status_s status; static struct actuator_armed_s armed; static struct safety_s safety; static struct vehicle_control_mode_s control_mode; -static struct offboard_control_setpoint_s sp_offboard; +static struct offboard_control_mode_s offboard_control_mode; /* tasks waiting for low prio thread */ typedef enum { @@ -1016,8 +1016,8 @@ int commander_thread_main(int argc, char *argv[]) memset(&sp_man, 0, sizeof(sp_man)); /* Subscribe to offboard control data */ - int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - memset(&sp_offboard, 0, sizeof(sp_offboard)); + int offboard_control_mode_sub = orb_subscribe(ORB_ID(offboard_control_mode)); + memset(&offboard_control_mode, 0, sizeof(offboard_control_mode)); /* Subscribe to telemetry status topics */ int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM]; @@ -1227,14 +1227,14 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); } - orb_check(sp_offboard_sub, &updated); + orb_check(offboard_control_mode_sub, &updated); if (updated) { - orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); + orb_copy(ORB_ID(offboard_control_mode), offboard_control_mode_sub, &offboard_control_mode); } - if (sp_offboard.timestamp != 0 && - sp_offboard.timestamp + OFFBOARD_TIMEOUT > hrt_absolute_time()) { + if (offboard_control_mode.timestamp != 0 && + offboard_control_mode.timestamp + OFFBOARD_TIMEOUT > hrt_absolute_time()) { if (status.offboard_control_signal_lost) { status.offboard_control_signal_lost = false; status_changed = true; @@ -2051,7 +2051,7 @@ int commander_thread_main(int argc, char *argv[]) led_deinit(); buzzer_deinit(); close(sp_man_sub); - close(sp_offboard_sub); + close(offboard_control_mode_sub); close(local_position_sub); close(global_position_sub); close(gps_sub); @@ -2426,56 +2426,30 @@ set_control_mode() control_mode.flag_control_auto_enabled = false; control_mode.flag_control_offboard_enabled = true; - switch (sp_offboard.mode) { - case OFFBOARD_CONTROL_MODE_DIRECT_RATES: - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = false; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - break; + /* + * The control flags depend on what is ignored according to the offboard control mode topic + * Inner loop flags (e.g. attitude) also depend on outer loop ignore flags (e.g. position) + */ + control_mode.flag_control_rates_enabled = !offboard_control_mode.ignore_bodyrate || + !offboard_control_mode.ignore_attitude || + !offboard_control_mode.ignore_position || + !offboard_control_mode.ignore_velocity || + !offboard_control_mode.ignore_acceleration_force; - case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - break; + control_mode.flag_control_attitude_enabled = !offboard_control_mode.ignore_attitude || + !offboard_control_mode.ignore_position || + !offboard_control_mode.ignore_velocity || + !offboard_control_mode.ignore_acceleration_force; - case OFFBOARD_CONTROL_MODE_DIRECT_FORCE: - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = false; - control_mode.flag_control_force_enabled = true; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - break; + control_mode.flag_control_velocity_enabled = !offboard_control_mode.ignore_velocity || + !offboard_control_mode.ignore_position; - case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED: - case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED: - case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED: - case OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED: - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = true; - control_mode.flag_control_velocity_enabled = true; - //XXX: the flags could depend on sp_offboard.ignore - break; + control_mode.flag_control_climb_rate_enabled = !offboard_control_mode.ignore_velocity || + !offboard_control_mode.ignore_position; - default: - control_mode.flag_control_rates_enabled = false; - control_mode.flag_control_attitude_enabled = false; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - } + control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position; + + control_mode.flag_control_altitude_enabled = !offboard_control_mode.ignore_position; break; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 6e49bd1d1..5daeae477 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -520,7 +520,7 @@ FixedwingAttitudeControl::vehicle_control_mode_poll() orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated); if (vcontrol_mode_updated) { - + orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode); } } @@ -795,10 +795,10 @@ FixedwingAttitudeControl::task_main() /* Simple handling of failsafe: deploy parachute if failsafe is on */ if (_vcontrol_mode.flag_control_termination_enabled) { _actuators_airframe.control[7] = 1.0f; -// warnx("_actuators_airframe.control[1] = 1.0f;"); + //warnx("_actuators_airframe.control[1] = 1.0f;"); } else { _actuators_airframe.control[7] = 0.0f; -// warnx("_actuators_airframe.control[1] = -1.0f;"); + //warnx("_actuators_airframe.control[1] = -1.0f;"); } /* decide if in stabilized or full manual control */ @@ -1077,20 +1077,25 @@ FixedwingAttitudeControl::task_main() _actuators_airframe.timestamp = hrt_absolute_time(); _actuators_airframe.timestamp_sample = _att.timestamp; - /* publish the actuator controls */ - if (_actuators_0_pub > 0) { - orb_publish(_actuators_id, _actuators_0_pub, &_actuators); - } else if (_actuators_id) { - _actuators_0_pub= orb_advertise(_actuators_id, &_actuators); - } + /* Only publish if any of the proper modes are enabled */ + if(_vcontrol_mode.flag_control_rates_enabled || + _vcontrol_mode.flag_control_attitude_enabled) + { + /* publish the actuator controls */ + if (_actuators_0_pub > 0) { + orb_publish(_actuators_id, _actuators_0_pub, &_actuators); + } else if (_actuators_id) { + _actuators_0_pub= orb_advertise(_actuators_id, &_actuators); + } - if (_actuators_2_pub > 0) { - /* publish the actuator controls*/ - orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe); + if (_actuators_2_pub > 0) { + /* publish the actuator controls*/ + orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe); - } else { - /* advertise and publish */ - _actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe); + } else { + /* advertise and publish */ + _actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe); + } } } diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 9711d8fc3..7d6b60e22 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -51,7 +51,6 @@ #include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/home_position.h> #include <uORB/topics/vehicle_status.h> -#include <uORB/topics/offboard_control_setpoint.h> #include <uORB/topics/vehicle_command.h> #include <uORB/topics/vehicle_local_position_setpoint.h> #include <uORB/topics/vehicle_vicon_position.h> diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 90c3cb47f..bce93cc6a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -108,7 +108,8 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _cmd_pub(-1), _flow_pub(-1), _range_pub(-1), - _offboard_control_sp_pub(-1), + _offboard_control_mode_pub(-1), + _actuator_controls_pub(-1), _global_vel_sp_pub(-1), _att_sp_pub(-1), _rates_sp_pub(-1), @@ -170,6 +171,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_set_attitude_target(msg); break; + case MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET: + handle_message_set_actuator_control_target(msg); + break; + case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE: handle_message_vision_position_estimate(msg); break; @@ -517,8 +522,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 +532,22 @@ 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)); - - /* 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; - } + 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); - /* 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_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_mode.timestamp = hrt_absolute_time(); - 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 +559,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 +581,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)) { - 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]; + /* set the local pos values */ + if (!offboard_control_mode.ignore_position) { + pos_sp_triplet.current.position_valid = true; + 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; @@ -699,6 +652,66 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t } void +MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *msg) +{ + mavlink_set_actuator_control_target_t set_actuator_control_target; + mavlink_msg_set_actuator_control_target_decode(msg, &set_actuator_control_target); + + struct offboard_control_mode_s offboard_control_mode; + memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));//XXX breaks compatibility with multiple setpoints + + struct actuator_controls_s actuator_controls; + memset(&actuator_controls, 0, sizeof(actuator_controls));//XXX breaks compatibility with multiple setpoints + + if ((mavlink_system.sysid == set_actuator_control_target.target_system || + set_actuator_control_target.target_system == 0) && + (mavlink_system.compid == set_actuator_control_target.target_component || + set_actuator_control_target.target_component == 0)) { + + /* ignore all since we are setting raw actuators here */ + offboard_control_mode.ignore_thrust = true; + offboard_control_mode.ignore_attitude = true; + offboard_control_mode.ignore_bodyrate = true; + offboard_control_mode.ignore_position = true; + offboard_control_mode.ignore_velocity = true; + offboard_control_mode.ignore_acceleration_force = true; + + offboard_control_mode.timestamp = hrt_absolute_time(); + + 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_mode), _offboard_control_mode_pub, &offboard_control_mode); + } + + + /* If we are in offboard control mode, publish the actuator controls */ + bool updated; + orb_check(_control_mode_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + } + + if (_control_mode.flag_control_offboard_enabled) { + + actuator_controls.timestamp = hrt_absolute_time(); + + /* Set duty cycles for the servos in actuator_controls_0 */ + for(size_t i = 0; i < 8; i++) { + actuator_controls.control[i] = set_actuator_control_target.controls[i]; + } + + if (_actuator_controls_pub < 0) { + _actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_0), &actuator_controls); + } else { + orb_publish(ORB_ID(actuator_controls_0), _actuator_controls_pub, &actuator_controls); + } + } + } + +} + +void MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) { mavlink_vision_position_estimate_t pos; @@ -743,42 +756,52 @@ 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 + static struct offboard_control_mode_s 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; - } + /* 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_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6)); + + /* + * The tricky part in pasrsing this message is that the offboard sender can set attitude and thrust + * using different messages. Eg.: First send set_attitude_target containing the attitude and ignore + * bits set for everything else and then send set_attitude_target containing the thrust and ignore bits + * set for everything else. + */ + + /* + * if attitude or body rate have been used (not ignored) previously and this message only sends + * throttle and has the ignore bits set for attitude and rates don't change the flags for attitude and + * body rates to keep the controllers running + */ + bool ignore_bodyrate = (bool)(set_attitude_target.type_mask & 0x7); + bool ignore_attitude = (bool)(set_attitude_target.type_mask & (1 << 7)); + + if (ignore_bodyrate && ignore_attitude && !offboard_control_mode.ignore_thrust) { + /* Message want's us to ignore everything except thrust: only ignore if previously ignored */ + offboard_control_mode.ignore_bodyrate = ignore_bodyrate && offboard_control_mode.ignore_bodyrate; + offboard_control_mode.ignore_attitude = ignore_attitude && offboard_control_mode.ignore_attitude; + } else { + offboard_control_mode.ignore_bodyrate = ignore_bodyrate; + offboard_control_mode.ignore_attitude = ignore_attitude; + } + offboard_control_mode.ignore_position = true; + offboard_control_mode.ignore_velocity = true; + offboard_control_mode.ignore_acceleration_force = true; - offboard_control_sp.timestamp = hrt_absolute_time(); - offboard_control_sp.mode =OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; //XXX handle rate control mode + offboard_control_mode.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 @@ -793,15 +816,16 @@ 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))) { - struct vehicle_attitude_setpoint_s att_sp; + if (!(offboard_control_mode.ignore_attitude)) { + static struct vehicle_attitude_setpoint_s att_sp = {}; att_sp.timestamp = hrt_absolute_time(); mavlink_quaternion_to_euler(set_attitude_target.q, &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body); att_sp.R_valid = true; - att_sp.thrust = set_attitude_target.thrust; + if (!offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid + att_sp.thrust = set_attitude_target.thrust; + } att_sp.yaw_sp_move_rate = 0.0; memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d)); att_sp.q_d_valid = true; @@ -814,14 +838,15 @@ 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))) { - struct vehicle_rates_setpoint_s rates_sp; + if (!(offboard_control_mode.ignore_bodyrate)) { + static struct vehicle_rates_setpoint_s rates_sp = {}; rates_sp.timestamp = hrt_absolute_time(); rates_sp.roll = set_attitude_target.body_roll_rate; rates_sp.pitch = set_attitude_target.body_pitch_rate; rates_sp.yaw = set_attitude_target.body_yaw_rate; - rates_sp.thrust = set_attitude_target.thrust; + if (!offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid + rates_sp.thrust = set_attitude_target.thrust; + } if (_att_sp_pub < 0) { _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 699996860..4886bbd0a 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -53,7 +53,7 @@ #include <uORB/topics/vehicle_land_detected.h> #include <uORB/topics/home_position.h> #include <uORB/topics/vehicle_status.h> -#include <uORB/topics/offboard_control_setpoint.h> +#include <uORB/topics/offboard_control_mode.h> #include <uORB/topics/vehicle_command.h> #include <uORB/topics/vehicle_local_position_setpoint.h> #include <uORB/topics/vehicle_global_velocity_setpoint.h> @@ -122,6 +122,7 @@ private: void handle_message_vision_position_estimate(mavlink_message_t *msg); void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg); void handle_message_set_position_target_local_ned(mavlink_message_t *msg); + void handle_message_set_actuator_control_target(mavlink_message_t *msg); void handle_message_set_attitude_target(mavlink_message_t *msg); void handle_message_radio_status(mavlink_message_t *msg); void handle_message_manual_control(mavlink_message_t *msg); @@ -142,7 +143,7 @@ private: /** * Exponential moving average filter to smooth time offset */ - void smooth_time_offset(uint64_t offset_ns); + void smooth_time_offset(uint64_t offset_ns); mavlink_status_t status; struct vehicle_local_position_s hil_local_pos; @@ -162,7 +163,8 @@ private: orb_advert_t _cmd_pub; orb_advert_t _flow_pub; orb_advert_t _range_pub; - orb_advert_t _offboard_control_sp_pub; + orb_advert_t _offboard_control_mode_pub; + orb_advert_t _actuator_controls_pub; orb_advert_t _global_vel_sp_pub; orb_advert_t _att_sp_pub; orb_advert_t _rates_sp_pub; diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index f60aa8d86..dbed29774 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -163,8 +163,8 @@ ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s); #include "topics/vehicle_control_debug.h" ORB_DEFINE(vehicle_control_debug, struct vehicle_control_debug_s); -#include "topics/offboard_control_setpoint.h" -ORB_DEFINE(offboard_control_setpoint, struct offboard_control_setpoint_s); +#include "topics/offboard_control_mode.h" +ORB_DEFINE(offboard_control_mode, struct offboard_control_mode_s); #include "topics/optical_flow.h" ORB_DEFINE(optical_flow, struct optical_flow_s); diff --git a/src/modules/uORB/topics/offboard_control_mode.h b/src/modules/uORB/topics/offboard_control_mode.h new file mode 100644 index 000000000..559659a1d --- /dev/null +++ b/src/modules/uORB/topics/offboard_control_mode.h @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2015 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file offboard_control_mode.h + * Definition of the manual_control_setpoint uORB topic. + */ + +#ifndef TOPIC_OFFBOARD_CONTROL_MODE_H_ +#define TOPIC_OFFBOARD_CONTROL_MODE_H_ + +#include <stdint.h> +#include "../uORB.h" + +/** + * Off-board control mode + */ + +/** + * @addtogroup topics + * @{ + */ + +struct offboard_control_mode_s { + uint64_t timestamp; + + bool ignore_thrust; + bool ignore_attitude; + bool ignore_bodyrate; + bool ignore_position; + bool ignore_velocity; + bool ignore_acceleration_force; + +}; /**< offboard control inputs */ +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(offboard_control_mode); + +#endif diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h deleted file mode 100644 index 72a28e501..000000000 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ /dev/null @@ -1,276 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file offboard_control_setpoint.h - * Definition of the manual_control_setpoint uORB topic. - */ - -#ifndef TOPIC_OFFBOARD_CONTROL_SETPOINT_H_ -#define TOPIC_OFFBOARD_CONTROL_SETPOINT_H_ - -#include <stdint.h> -#include "../uORB.h" - -/** - * Off-board control inputs. - * - * Typically sent by a ground control station / joystick or by - * some off-board controller via C or SIMULINK. - */ -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_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, -}; - -/** - * @addtogroup topics - * @{ - */ - -struct offboard_control_setpoint_s { - uint64_t timestamp; - - enum OFFBOARD_CONTROL_MODE mode; /**< The current control inputs mode */ - - 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; - - float aux1_cam_pan_flaps; - float aux2_cam_tilt; - float aux3_cam_zoom; - float aux4_cam_roll; - -}; /**< offboard control inputs */ -/** - * @} - */ - -/** - * 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); - -#endif diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 288ba2ebe..74e1efd6c 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -782,18 +782,23 @@ void VtolAttitudeControl::task_main() fill_mc_att_control_output(); fill_mc_att_rates_sp(); - if (_actuators_0_pub > 0) { - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); - - } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); - } - - if (_actuators_1_pub > 0) { - orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); - - } else { - _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); + /* Only publish if the proper mode(s) are enabled */ + if(_v_control_mode.flag_control_attitude_enabled || + _v_control_mode.flag_control_rates_enabled) + { + if (_actuators_0_pub > 0) { + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); + + } else { + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); + } + + if (_actuators_1_pub > 0) { + orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); + + } else { + _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); + } } } } @@ -813,18 +818,23 @@ void VtolAttitudeControl::task_main() fill_fw_att_control_output(); fill_fw_att_rates_sp(); - if (_actuators_0_pub > 0) { - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); + /* Only publish if the proper mode(s) are enabled */ + if(_v_control_mode.flag_control_attitude_enabled || + _v_control_mode.flag_control_rates_enabled) + { + if (_actuators_0_pub > 0) { + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); - } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); - } + } else { + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); + } - if (_actuators_1_pub > 0) { - orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); + if (_actuators_1_pub > 0) { + orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); - } else { - _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); + } else { + _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); + } } } } |