From 2b2f7e9407a551682fc76401462e08df9871cc98 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 13 Feb 2015 23:19:43 +0100 Subject: 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 --- src/modules/commander/commander.cpp | 85 +++---- src/modules/mavlink/mavlink_messages.cpp | 1 - src/modules/mavlink/mavlink_receiver.cpp | 171 ++++--------- src/modules/mavlink/mavlink_receiver.h | 6 +- src/modules/uORB/objects_common.cpp | 4 +- src/modules/uORB/topics/offboard_control_mode.h | 73 ++++++ .../uORB/topics/offboard_control_setpoint.h | 276 --------------------- 7 files changed, 162 insertions(+), 454 deletions(-) create mode 100644 src/modules/uORB/topics/offboard_control_mode.h delete mode 100644 src/modules/uORB/topics/offboard_control_setpoint.h (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 242d8a486..5a362666c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -65,7 +65,7 @@ #include #include #include -#include +#include #include #include #include @@ -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,31 @@ 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; - 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_velocity_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_climb_rate_enabled = !offboard_control_mode.ignore_velocity || + !offboard_control_mode.ignore_position; + + 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/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 #include #include -#include #include #include #include diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 90c3cb47f..3a5ae47ab 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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)); - - /* 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_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); - 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_sp.timestamp = hrt_absolute_time(); + 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 @@ -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_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,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; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 699996860..15943753d 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -53,7 +53,7 @@ #include #include #include -#include +#include #include #include #include @@ -142,7 +142,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 +162,7 @@ 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 _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 + * + * 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 +#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 - * - * 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 -#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 -- cgit v1.2.3 From 3d29fa5f4e387457ad425425a70c3a0792f808e7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 15 Feb 2015 19:22:47 +0100 Subject: mavlink receiver: fix indentation --- src/modules/mavlink/mavlink_receiver.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 3a5ae47ab..3e09e5c81 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -580,10 +580,10 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* 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; + 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; } -- cgit v1.2.3 From 0389d30e0ee07b7d3d81da99dacfc1d5854eed26 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 16 Feb 2015 21:47:11 +0100 Subject: offboard attitude sp: handle thrust only messages if attitude/rates haven been used previously do not set the ignore flags even if the message asks us to do so to keep the controllers running --- src/modules/mavlink/mavlink_receiver.cpp | 23 ++++++++++++++++++----- 1 file changed, 18 insertions(+), 5 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 3e09e5c81..abaec51e4 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -693,7 +693,7 @@ 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_mode_s offboard_control_mode; + static 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 */ @@ -702,12 +702,25 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) (mavlink_system.compid == set_attitude_target.target_component || set_attitude_target.target_component == 0)) { - /* set correct ignore flags for body rate fields: copy from mavlink message */ - 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_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6)); - /* set correct ignore flags for attitude field: copy from mavlink message */ - offboard_control_mode.ignore_attitude = (bool)(set_attitude_target.type_mask & (1 << 7)); + + /* + * 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.timestamp = hrt_absolute_time(); -- cgit v1.2.3 From ed16bd6fc6c2d4e8c080c9c9f1e43eda854acc4d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 17 Feb 2015 20:35:28 +0100 Subject: mavlinkreceiver: set att target: remove memset --- src/modules/mavlink/mavlink_receiver.cpp | 1 - 1 file changed, 1 deletion(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index abaec51e4..70bd390d8 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -694,7 +694,6 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) mavlink_msg_set_attitude_target_decode(msg, &set_attitude_target); static 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 || -- cgit v1.2.3 From ee6dc51ef252751cccbc5018576a27d8a1bb5cf4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 21 Feb 2015 11:08:34 +0100 Subject: improve offboard attitude setpoint handling --- src/modules/mavlink/mavlink_receiver.cpp | 29 +++++++++++++++++++++-------- 1 file changed, 21 insertions(+), 8 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 70bd390d8..67942912f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -704,6 +704,13 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) /* set correct ignore flags for thrust field: copy from mavlink message */ 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 @@ -721,6 +728,10 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) 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_mode.timestamp = hrt_absolute_time(); if (_offboard_control_mode_pub < 0) { @@ -742,15 +753,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_mode.ignore_attitude || - offboard_control_mode.ignore_thrust)) { - 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; @@ -763,14 +775,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_mode.ignore_bodyrate || - offboard_control_mode.ignore_thrust)) { - 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); -- cgit v1.2.3 From 40ae0ebdaca797ef9265c00ebb7f38eb82df7336 Mon Sep 17 00:00:00 2001 From: Matt Beall Date: Thu, 19 Feb 2015 15:17:52 -0700 Subject: Set up to receive mavlink actuator control messages and publish to uorb --- src/modules/mavlink/mavlink_receiver.cpp | 29 +++++++++++++++++++++++++++++ src/modules/mavlink/mavlink_receiver.h | 2 ++ 2 files changed, 31 insertions(+) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 67942912f..86dff69c5 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -109,6 +109,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _flow_pub(-1), _range_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; @@ -648,6 +653,30 @@ 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 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)){ + + actuator_controls.timestamp = hrt_absolute_time(); + + for(size_t i = 0; i < 8 ; i++){ + actuator_controls.control[i] = set_actuator_control_target.controls[i]; + } + _actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_0), &actuator_controls); + } + +} + void MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 15943753d..4886bbd0a 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.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); @@ -163,6 +164,7 @@ private: orb_advert_t _flow_pub; orb_advert_t _range_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; -- cgit v1.2.3 From 877232119fe78af70a6a096f7d3a3deec2d825fe Mon Sep 17 00:00:00 2001 From: Matt Beall Date: Tue, 24 Feb 2015 12:29:51 -0700 Subject: Small changes --- src/modules/fw_att_control/fw_att_control_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src') 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..5052b41c3 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) { - + //vehicle_control_mode_s 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 */ -- cgit v1.2.3 From 5e199b3984e35e6a8078abbc36d9c9440e1cd7af Mon Sep 17 00:00:00 2001 From: Matt Beall Date: Tue, 24 Feb 2015 13:31:22 -0700 Subject: Set ignore flags to true --- src/modules/mavlink/mavlink_receiver.cpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 86dff69c5..7c506e005 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -667,6 +667,24 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m (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(O + actuator_controls.timestamp = RB_ID(offboard_control_mode), &offboard_control_mode); + + } else { + orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode); + } + actuator_controls.timestamp = hrt_absolute_time(); for(size_t i = 0; i < 8 ; i++){ -- cgit v1.2.3 From e2de72b882d7c74cdaafcfc74ab7176ef94a4455 Mon Sep 17 00:00:00 2001 From: Matt Beall Date: Tue, 24 Feb 2015 16:43:49 -0700 Subject: Added offboard actuator controls flags to offboard control mode and vehicle control mode to disable controls in att_control apps --- msg/vehicle_control_mode.msg | 1 + src/modules/commander/commander.cpp | 9 ++++ src/modules/fw_att_control/fw_att_control_main.cpp | 28 ++++++------ src/modules/mavlink/mavlink_receiver.cpp | 7 ++- src/modules/mc_att_control/mc_att_control_main.cpp | 2 +- src/modules/uORB/topics/offboard_control_mode.h | 1 + .../vtol_att_control/vtol_att_control_main.cpp | 50 +++++++++++++--------- 7 files changed, 60 insertions(+), 38 deletions(-) (limited to 'src') diff --git a/msg/vehicle_control_mode.msg b/msg/vehicle_control_mode.msg index 153a642bb..44f018642 100644 --- a/msg/vehicle_control_mode.msg +++ b/msg/vehicle_control_mode.msg @@ -12,6 +12,7 @@ bool flag_system_hil_enabled bool flag_control_manual_enabled # true if manual input is mixed in bool flag_control_auto_enabled # true if onboard autopilot should act bool flag_control_offboard_enabled # true if offboard control should be used +bool flag_control_offboard_actuator_control_enabled #true if raw actuator control in offboard used bool flag_control_rates_enabled # true if rates are stabilized bool flag_control_attitude_enabled # true if attitude stabilization is mixed in bool flag_control_force_enabled # true if force control is mixed in diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5a362666c..e688b8fa5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2302,6 +2302,7 @@ set_control_mode() control_mode.flag_external_manual_override_ok = (!status.is_rotary_wing && !status.is_vtol); control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON; control_mode.flag_control_offboard_enabled = false; + control_mode.flag_control_offboard_actuator_control_enabled = false; switch (status.nav_state) { case vehicle_status_s::NAVIGATION_STATE_MANUAL: @@ -2452,6 +2453,14 @@ set_control_mode() control_mode.flag_control_altitude_enabled = !offboard_control_mode.ignore_position; + control_mode.flag_control_offboard_actuator_control_enabled = offboard_control_mode.ignore_thrust && + offboard_control_mode.ignore_attitude && + offboard_control_mode.ignore_bodyrate && + offboard_control_mode.ignore_position && + offboard_control_mode.ignore_velocity && + offboard_control_mode.ignore_acceleration_force && + offboard_control_mode.actuator_control_mode; + break; default: 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 5052b41c3..a7b563b0f 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -1077,20 +1077,24 @@ 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 actuator_control mode is not enabled */ + if(!_vcontrol_mode.flag_control_offboard_actuator_control_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_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7c506e005..48866f036 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -540,7 +540,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t 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); - + offboard_control_mode.actuator_control_mode = false; offboard_control_mode.timestamp = hrt_absolute_time(); @@ -678,9 +678,7 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m offboard_control_mode.timestamp = hrt_absolute_time(); if (_offboard_control_mode_pub < 0) { - _offboard_control_mode_pub = orb_advertise(O - actuator_controls.timestamp = RB_ID(offboard_control_mode), &offboard_control_mode); - + _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); } @@ -774,6 +772,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) offboard_control_mode.ignore_bodyrate = ignore_bodyrate; offboard_control_mode.ignore_attitude = ignore_attitude; } + offboard_control_mode.actuator_control_mode = false; offboard_control_mode.ignore_position = true; offboard_control_mode.ignore_velocity = true; diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index a0d76b0a6..099b92404 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -804,7 +804,7 @@ MulticopterAttitudeControl::task_main() _actuators.timestamp = hrt_absolute_time(); _actuators.timestamp_sample = _v_att.timestamp; - if (!_actuators_0_circuit_breaker_enabled) { + if (!_actuators_0_circuit_breaker_enabled || !_v_control_mode.flag_control_offboard_actuator_control_enabled) { if (_actuators_0_pub > 0) { orb_publish(_actuators_id, _actuators_0_pub, &_actuators); perf_end(_controller_latency_perf); diff --git a/src/modules/uORB/topics/offboard_control_mode.h b/src/modules/uORB/topics/offboard_control_mode.h index 559659a1d..956c3881e 100644 --- a/src/modules/uORB/topics/offboard_control_mode.h +++ b/src/modules/uORB/topics/offboard_control_mode.h @@ -61,6 +61,7 @@ struct offboard_control_mode_s { bool ignore_position; bool ignore_velocity; bool ignore_acceleration_force; + bool actuator_control_mode; }; /**< offboard control inputs */ /** 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..53a08f97a 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,22 @@ 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 actuator_control mode is not enabled */ + if(!_v_control_mode.flag_control_offboard_actuator_control_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 +817,22 @@ 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 actuator_control mode is not enabled */ + if(!_v_control_mode.flag_control_offboard_actuator_control_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); + } } } } -- cgit v1.2.3 From fd4fd4eaaa39330d4a1cf1154100e6d1201e847f Mon Sep 17 00:00:00 2001 From: Matt Beall Date: Tue, 24 Feb 2015 16:52:50 -0700 Subject: Set the actuator control flag in receiver --- src/modules/mavlink/mavlink_receiver.cpp | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 48866f036..8291f2006 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -674,6 +674,8 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m offboard_control_mode.ignore_position = true; offboard_control_mode.ignore_velocity = true; offboard_control_mode.ignore_acceleration_force = true; + + offboard_control_mode.actuator_control_mode = false; offboard_control_mode.timestamp = hrt_absolute_time(); -- cgit v1.2.3 From 7d6723aa2d088e7527d043704a2fc6b9297d248f Mon Sep 17 00:00:00 2001 From: Matt Beall Date: Tue, 24 Feb 2015 16:53:47 -0700 Subject: small change --- src/modules/mavlink/mavlink_receiver.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 8291f2006..eb1af79d6 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -674,8 +674,8 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m offboard_control_mode.ignore_position = true; offboard_control_mode.ignore_velocity = true; offboard_control_mode.ignore_acceleration_force = true; - - offboard_control_mode.actuator_control_mode = false; + + offboard_control_mode.actuator_control_mode = true; offboard_control_mode.timestamp = hrt_absolute_time(); -- cgit v1.2.3 From b8aa79cae4da9343a0ab301c6383c6ed0dfc61db Mon Sep 17 00:00:00 2001 From: Matt Beall Date: Wed, 25 Feb 2015 11:54:41 -0700 Subject: Made changes to have actuator controls mirror other syntax more closely --- src/modules/mavlink/mavlink_receiver.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index eb1af79d6..c551fdfea 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -690,7 +690,13 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m for(size_t i = 0; i < 8 ; i++){ actuator_controls.control[i] = set_actuator_control_target.controls[i]; } - _actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_0), &actuator_controls); + + if (_offboard_control_mode_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); + } + } } -- cgit v1.2.3 From b0e878dbae1f471ddf0d3c063f238260124f9d5a Mon Sep 17 00:00:00 2001 From: Matt Beall Date: Wed, 25 Feb 2015 12:00:23 -0700 Subject: Compiler error --- src/modules/mavlink/mavlink_receiver.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c551fdfea..a4c6c3da4 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -659,9 +659,12 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m 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 || -- cgit v1.2.3 From d7dc3a3ee85e3a3c28d97ef49d53026f254d3650 Mon Sep 17 00:00:00 2001 From: Matt Beall Date: Wed, 25 Feb 2015 12:12:14 -0700 Subject: Cleaned up some chunky code --- src/modules/commander/commander.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e688b8fa5..024e6c18a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2453,13 +2453,13 @@ set_control_mode() control_mode.flag_control_altitude_enabled = !offboard_control_mode.ignore_position; - control_mode.flag_control_offboard_actuator_control_enabled = offboard_control_mode.ignore_thrust && - offboard_control_mode.ignore_attitude && - offboard_control_mode.ignore_bodyrate && - offboard_control_mode.ignore_position && - offboard_control_mode.ignore_velocity && - offboard_control_mode.ignore_acceleration_force && - offboard_control_mode.actuator_control_mode; + control_mode.flag_control_offboard_actuator_control_enabled = offboard_control_mode.ignore_thrust && + offboard_control_mode.ignore_attitude && + offboard_control_mode.ignore_bodyrate && + offboard_control_mode.ignore_position && + offboard_control_mode.ignore_velocity && + offboard_control_mode.ignore_acceleration_force && + offboard_control_mode.actuator_control_mode; break; -- cgit v1.2.3 From 220fb19eb74b330bd3e97efc628aeb1bda510dcf Mon Sep 17 00:00:00 2001 From: Matt Beall Date: Wed, 25 Feb 2015 14:25:28 -0700 Subject: Removed actuator_control_mode flags...Using pre-existing flags instead --- msg/vehicle_control_mode.msg | 1 - src/modules/commander/commander.cpp | 10 ---------- src/modules/fw_att_control/fw_att_control_main.cpp | 7 ++++--- src/modules/mavlink/mavlink_receiver.cpp | 18 +++++++----------- src/modules/mc_att_control/mc_att_control_main.cpp | 2 +- src/modules/uORB/topics/offboard_control_mode.h | 1 - src/modules/vtol_att_control/vtol_att_control_main.cpp | 10 ++++++---- 7 files changed, 18 insertions(+), 31 deletions(-) (limited to 'src') diff --git a/msg/vehicle_control_mode.msg b/msg/vehicle_control_mode.msg index 44f018642..153a642bb 100644 --- a/msg/vehicle_control_mode.msg +++ b/msg/vehicle_control_mode.msg @@ -12,7 +12,6 @@ bool flag_system_hil_enabled bool flag_control_manual_enabled # true if manual input is mixed in bool flag_control_auto_enabled # true if onboard autopilot should act bool flag_control_offboard_enabled # true if offboard control should be used -bool flag_control_offboard_actuator_control_enabled #true if raw actuator control in offboard used bool flag_control_rates_enabled # true if rates are stabilized bool flag_control_attitude_enabled # true if attitude stabilization is mixed in bool flag_control_force_enabled # true if force control is mixed in diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 024e6c18a..2c5e0aa88 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2302,7 +2302,6 @@ set_control_mode() control_mode.flag_external_manual_override_ok = (!status.is_rotary_wing && !status.is_vtol); control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON; control_mode.flag_control_offboard_enabled = false; - control_mode.flag_control_offboard_actuator_control_enabled = false; switch (status.nav_state) { case vehicle_status_s::NAVIGATION_STATE_MANUAL: @@ -2442,7 +2441,6 @@ set_control_mode() !offboard_control_mode.ignore_velocity || !offboard_control_mode.ignore_acceleration_force; - control_mode.flag_control_velocity_enabled = !offboard_control_mode.ignore_velocity || !offboard_control_mode.ignore_position; @@ -2453,14 +2451,6 @@ set_control_mode() control_mode.flag_control_altitude_enabled = !offboard_control_mode.ignore_position; - control_mode.flag_control_offboard_actuator_control_enabled = offboard_control_mode.ignore_thrust && - offboard_control_mode.ignore_attitude && - offboard_control_mode.ignore_bodyrate && - offboard_control_mode.ignore_position && - offboard_control_mode.ignore_velocity && - offboard_control_mode.ignore_acceleration_force && - offboard_control_mode.actuator_control_mode; - break; default: 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 a7b563b0f..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) { - //vehicle_control_mode_s + orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode); } } @@ -1077,8 +1077,9 @@ FixedwingAttitudeControl::task_main() _actuators_airframe.timestamp = hrt_absolute_time(); _actuators_airframe.timestamp_sample = _att.timestamp; - /* Only publish if actuator_control mode is not enabled */ - if(!_vcontrol_mode.flag_control_offboard_actuator_control_enabled) + /* 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) { diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index a4c6c3da4..27a6d4bca 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -540,8 +540,6 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t 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); - offboard_control_mode.actuator_control_mode = false; - offboard_control_mode.timestamp = hrt_absolute_time(); @@ -665,10 +663,10 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m 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)){ + 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; @@ -678,8 +676,6 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m offboard_control_mode.ignore_velocity = true; offboard_control_mode.ignore_acceleration_force = true; - offboard_control_mode.actuator_control_mode = true; - offboard_control_mode.timestamp = hrt_absolute_time(); if (_offboard_control_mode_pub < 0) { @@ -690,11 +686,12 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m actuator_controls.timestamp = hrt_absolute_time(); - for(size_t i = 0; i < 8 ; i++){ + /* 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 (_offboard_control_mode_pub < 0) { + 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); @@ -783,7 +780,6 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) offboard_control_mode.ignore_bodyrate = ignore_bodyrate; offboard_control_mode.ignore_attitude = ignore_attitude; } - offboard_control_mode.actuator_control_mode = false; offboard_control_mode.ignore_position = true; offboard_control_mode.ignore_velocity = true; diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 099b92404..a0d76b0a6 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -804,7 +804,7 @@ MulticopterAttitudeControl::task_main() _actuators.timestamp = hrt_absolute_time(); _actuators.timestamp_sample = _v_att.timestamp; - if (!_actuators_0_circuit_breaker_enabled || !_v_control_mode.flag_control_offboard_actuator_control_enabled) { + if (!_actuators_0_circuit_breaker_enabled) { if (_actuators_0_pub > 0) { orb_publish(_actuators_id, _actuators_0_pub, &_actuators); perf_end(_controller_latency_perf); diff --git a/src/modules/uORB/topics/offboard_control_mode.h b/src/modules/uORB/topics/offboard_control_mode.h index 956c3881e..559659a1d 100644 --- a/src/modules/uORB/topics/offboard_control_mode.h +++ b/src/modules/uORB/topics/offboard_control_mode.h @@ -61,7 +61,6 @@ struct offboard_control_mode_s { bool ignore_position; bool ignore_velocity; bool ignore_acceleration_force; - bool actuator_control_mode; }; /**< offboard control inputs */ /** 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 53a08f97a..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,8 +782,9 @@ void VtolAttitudeControl::task_main() fill_mc_att_control_output(); fill_mc_att_rates_sp(); - /* Only publish if actuator_control mode is not enabled */ - if(!_v_control_mode.flag_control_offboard_actuator_control_enabled) + /* 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); @@ -817,8 +818,9 @@ void VtolAttitudeControl::task_main() fill_fw_att_control_output(); fill_fw_att_rates_sp(); - /* Only publish if actuator_control mode is not enabled */ - if(!_v_control_mode.flag_control_offboard_actuator_control_enabled) + /* 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); -- cgit v1.2.3 From 0f51907dd662ae6ebc9ab997dfeda273769cffce Mon Sep 17 00:00:00 2001 From: Matt Beall Date: Wed, 25 Feb 2015 17:31:56 -0700 Subject: Check if offboard mode was activated before publishing controls --- src/modules/mavlink/mavlink_receiver.cpp | 28 +++++++++++++++++++--------- 1 file changed, 19 insertions(+), 9 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 27a6d4bca..bce93cc6a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -684,19 +684,29 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode); } - 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 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 (_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); + 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); + } } - } } -- cgit v1.2.3