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

Merge pull request #1151 from PX4/offboard2_externalsetpointmessages

Browse Source 
Offboard2: Handle external setpoint messages
This commit is contained in:
Lorenz Meier
2014-10-07 10:29:04 +02:00
parent d856356fce c6de36d683
commit 79e5ec6465
17 changed files with 571 additions and 240 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink.c
+7
View File
@@ -68,6 +68,13 @@ PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING);
* @group MAVLink
*/
PARAM_DEFINE_INT32(MAV_USEHILGPS, 0);
/**
* Forward external setpoint messages
* If set to 1 incomming external setpoint messages will be directly forwarded to the controllers if in offboard
* control mode
* @group MAVLink
*/
PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1);
mavlink_system_t mavlink_system = {
100,
src/modules/mavlink/mavlink_main.cpp
+7
View File
@@ -123,6 +123,7 @@ Mavlink::Mavlink() :
_task_running(false),
_hil_enabled(false),
_use_hil_gps(false),
_forward_externalsp(false),
_is_usb_uart(false),
_wait_to_transmit(false),
_received_messages(false),
@@ -483,6 +484,7 @@ void Mavlink::mavlink_update_system(void)
_param_component_id = param_find("MAV_COMP_ID");
_param_system_type = param_find("MAV_TYPE");
_param_use_hil_gps = param_find("MAV_USEHILGPS");
_param_forward_externalsp = param_find("MAV_FWDEXTSP");
}
/* update system and component id */
@@ -529,6 +531,11 @@ void Mavlink::mavlink_update_system(void)
param_get(_param_use_hil_gps, &use_hil_gps);
_use_hil_gps = (bool)use_hil_gps;
int32_t forward_externalsp;
param_get(_param_forward_externalsp, &forward_externalsp);
_forward_externalsp = (bool)forward_externalsp;
}
int Mavlink::get_system_id()
src/modules/mavlink/mavlink_main.h
+5 -1
View File
@@ -137,6 +137,8 @@ public:
bool get_use_hil_gps() { return _use_hil_gps; }
bool get_forward_externalsp() { return _forward_externalsp; }
bool get_flow_control_enabled() { return _flow_control_enabled; }
bool get_forwarding_on() { return _forwarding_on; }
@@ -232,7 +234,7 @@ public:
bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
bool message_buffer_write(const void *ptr, int size);
void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); }
@@ -275,6 +277,7 @@ private:
/* states */
bool _hil_enabled; /**< Hardware In the Loop mode */
bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
bool _forward_externalsp; /**< Forward external setpoint messages to controllers directly if in offboard mode */
bool _is_usb_uart; /**< Port is USB */
bool _wait_to_transmit; /**< Wait to transmit until received messages. */
bool _received_messages; /**< Whether we've received valid mavlink messages. */
@@ -349,6 +352,7 @@ private:
param_t _param_component_id;
param_t _param_system_type;
param_t _param_use_hil_gps;
param_t _param_forward_externalsp;
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _txerr_perf; /**< TX error counter */
src/modules/mavlink/mavlink_receiver.cpp
+291 -1
View File
@@ -37,6 +37,7 @@
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
/* XXX trim includes */
@@ -105,10 +106,11 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_flow_pub(-1),
_range_pub(-1),
_offboard_control_sp_pub(-1),
_local_pos_sp_pub(-1),
_global_vel_sp_pub(-1),
_att_sp_pub(-1),
_rates_sp_pub(-1),
_force_sp_pub(-1),
_pos_sp_triplet_pub(-1),
_vicon_position_pub(-1),
_vision_position_pub(-1),
_telemetry_status_pub(-1),
@@ -154,6 +156,14 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_vicon_position_estimate(msg);
break;
case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
handle_message_set_position_target_local_ned(msg);
break;
case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
handle_message_set_attitude_target(msg);
break;
case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
handle_message_vision_position_estimate(msg);
break;
@@ -474,6 +484,189 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
}
}
void
MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t *msg)
{
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
/* Only accept messages which are intended for this system */
if ((mavlink_system.sysid == set_position_target_local_ned.target_system ||
set_position_target_local_ned.target_system == 0) &&
(mavlink_system.compid == set_position_target_local_ned.target_component ||
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_sp.ignore &= ~(1 << OFB_IGN_BIT_YAW);
offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 10)) <<
OFB_IGN_BIT_YAW;
offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAWRATE);
offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 11)) <<
OFB_IGN_BIT_YAWRATE;
offboard_control_sp.timestamp = hrt_absolute_time();
if (_offboard_control_sp_pub < 0) {
_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
} else {
orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp);
}
/* If we are in offboard control mode and offboard control loop through is enabled
* also publish the setpoint topic which is read by the controller */
if (_mavlink->get_forward_externalsp()) {
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) {
if (offboard_control_sp.isForceSetpoint &&
offboard_control_sp_ignore_position_all(offboard_control_sp) &&
offboard_control_sp_ignore_velocity_all(offboard_control_sp)) {
/* 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];
//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 {
/* It's not a pure force setpoint: publish to setpoint triplet topic */
struct position_setpoint_triplet_s pos_sp_triplet;
pos_sp_triplet.previous.valid = false;
pos_sp_triplet.next.valid = false;
pos_sp_triplet.current.valid = true;
pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others
/* set the local pos values if the setpoint type is 'local pos' and none
* of the local pos fields is set to 'ignore' */
if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
!offboard_control_sp_ignore_position_some(offboard_control_sp)) {
pos_sp_triplet.current.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];
} 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)) {
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];
} 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)) {
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.acceleration_is_force =
offboard_control_sp.isForceSetpoint;
} else {
pos_sp_triplet.current.acceleration_valid = false;
}
/* set the yaw sp value if the setpoint type is 'local pos' and the yaw
* field is not set to 'ignore' */
if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
!offboard_control_sp_ignore_yaw(offboard_control_sp)) {
pos_sp_triplet.current.yaw_valid = true;
pos_sp_triplet.current.yaw = offboard_control_sp.yaw;
} else {
pos_sp_triplet.current.yaw_valid = false;
}
/* set the yawrate sp value if the setpoint type is 'local pos' and the yawrate
* field is not set to 'ignore' */
if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
!offboard_control_sp_ignore_yawrate(offboard_control_sp)) {
pos_sp_triplet.current.yawspeed_valid = true;
pos_sp_triplet.current.yawspeed = offboard_control_sp.yaw_rate;
} else {
pos_sp_triplet.current.yawspeed_valid = false;
}
//XXX handle global pos setpoints (different MAV frames)
if (_pos_sp_triplet_pub < 0) {
_pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet),
&pos_sp_triplet);
} else {
orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub,
&pos_sp_triplet);
}
}
}
}
}
}
void
MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
{
@@ -513,6 +706,103 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
}
}
void
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
/* 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_sp.timestamp = hrt_absolute_time();
offboard_control_sp.mode =OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; //XXX handle rate control mode
if (_offboard_control_sp_pub < 0) {
_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
} else {
orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp);
}
/* If we are in offboard control mode and offboard control loop through is enabled
* also publish the setpoint topic which is read by the controller */
if (_mavlink->get_forward_externalsp()) {
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) {
/* 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;
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, att_sp.R_body);
att_sp.R_valid = true;
att_sp.thrust = set_attitude_target.thrust;
memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d));
att_sp.q_d_valid = true;
if (_att_sp_pub < 0) {
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
} else {
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp);
}
}
/* 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;
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 (_att_sp_pub < 0) {
_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
} else {
orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp);
}
}
}
}
}
}
void
MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
{
src/modules/mavlink/mavlink_receiver.h
+5 -1
View File
@@ -71,6 +71,7 @@
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/vehicle_force_setpoint.h>
#include "mavlink_ftp.h"
@@ -117,6 +118,8 @@ private:
void handle_message_vicon_position_estimate(mavlink_message_t *msg);
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_attitude_target(mavlink_message_t *msg);
void handle_message_radio_status(mavlink_message_t *msg);
void handle_message_manual_control(mavlink_message_t *msg);
void handle_message_heartbeat(mavlink_message_t *msg);
@@ -145,10 +148,11 @@ private:
orb_advert_t _flow_pub;
orb_advert_t _range_pub;
orb_advert_t _offboard_control_sp_pub;
orb_advert_t _local_pos_sp_pub;
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 _pos_sp_triplet_pub;
orb_advert_t _vicon_position_pub;
orb_advert_t _vision_position_pub;
orb_advert_t _telemetry_status_pub;