From f4608707389dbc30eb25db524d6e008c8033d052 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 17 Jul 2014 09:11:57 +0200 Subject: support force setpoints --- src/modules/commander/commander.cpp | 9 ++++++++ src/modules/mavlink/mavlink_receiver.cpp | 24 +++++++++++++++++++++- src/modules/mavlink/mavlink_receiver.h | 2 ++ .../uORB/topics/offboard_control_setpoint.h | 7 ++++--- src/modules/uORB/topics/vehicle_control_mode.h | 1 + 5 files changed, 39 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index cc68c4e36..fc9560e18 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1951,6 +1951,15 @@ set_control_mode() control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; break; + 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: diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e5d380cfb..2cc2d6162 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -105,6 +105,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _global_vel_sp_pub(-1), _att_sp_pub(-1), _rates_sp_pub(-1), + _force_sp_pub(-1), _vicon_position_pub(-1), _telemetry_status_pub(-1), _rc_pub(-1), @@ -444,6 +445,13 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes offboard_control_sp.acceleration[1] = local_ned_position_setpoint_external.afy; offboard_control_sp.acceleration[2] = local_ned_position_setpoint_external.afz; offboard_control_sp.isForceSetpoint = (bool)(local_ned_position_setpoint_external.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 |= (local_ned_position_setpoint_external.type_mask & (1 << i)); @@ -467,9 +475,23 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes if (updated) { orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); } - if (_control_mode.flag_control_offboard_enabled) { + if (offboard_control_sp.isForceSetpoint) { + 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]; + //XXX: yaw + if (_force_sp_pub < 0) { + _force_sp_pub = orb_advertise(ORB_ID(vehicle_force_setpoint), &force_sp); + } else { + orb_publish(ORB_ID(vehicle_force_setpoint), _force_sp_pub, &force_sp); + } + } else { + //XXX: copy to and publish setpoint triplet here + } + } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 0f12cf32c..0044b42cb 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -70,6 +70,7 @@ #include #include #include +#include #include "mavlink_ftp.h" @@ -146,6 +147,7 @@ private: orb_advert_t _global_vel_sp_pub; orb_advert_t _att_sp_pub; orb_advert_t _rates_sp_pub; + orb_advert_t _force_sp_pub; orb_advert_t _vicon_position_pub; orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 809a2ace4..19f11ba92 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -58,9 +58,10 @@ enum OFFBOARD_CONTROL_MODE { OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED = 5, OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED = 6, OFFBOARD_CONTROL_MODE_DIRECT_GLOBAL = 7, - OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 8, - OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 9, - OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 10, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ + 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 { diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 49e2ba4b5..ca7705456 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -77,6 +77,7 @@ struct vehicle_control_mode_s { bool flag_control_offboard_enabled; /**< true if offboard control should be 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 */ bool flag_control_velocity_enabled; /**< true if horizontal velocity (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_control_altitude_enabled; /**< true if altitude is controlled */ -- cgit v1.2.3