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

Fix formatting

Browse Source
This commit is contained in:
M.H.Kabir
2014-12-16 14:08:28 +05:30
parent a5591b4791
commit f7da65f819
2 changed files with 502 additions and 413 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+422 -282
View File
File diff suppressed because it is too large Load Diff
src/modules/mavlink/mavlink_receiver.cpp
+80 -131
View File
@@ -88,42 +88,40 @@ static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f;
MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_mavlink(parent), _mavlink(parent),
status {}, status{},
hil_local_pos {}, hil_local_pos{},
_control_mode {}, _control_mode{},
_global_pos_pub(-1), _global_pos_pub(-1),
_local_pos_pub(-1), _local_pos_pub(-1),
_attitude_pub(-1), _attitude_pub(-1),
_gps_pub(-1), _gps_pub(-1),
_sensors_pub(-1), _sensors_pub(-1),
_gyro_pub(-1), _gyro_pub(-1),
_accel_pub(-1), _accel_pub(-1),
_mag_pub(-1), _mag_pub(-1),
_baro_pub(-1), _baro_pub(-1),
_airspeed_pub(-1), _airspeed_pub(-1),
_battery_pub(-1), _battery_pub(-1),
_cmd_pub(-1), _cmd_pub(-1),
_flow_pub(-1), _flow_pub(-1),
_range_pub(-1), _range_pub(-1),
_offboard_control_sp_pub(-1), _offboard_control_sp_pub(-1),
_global_vel_sp_pub(-1), _global_vel_sp_pub(-1),
_att_sp_pub(-1), _att_sp_pub(-1),
_rates_sp_pub(-1), _rates_sp_pub(-1),
_force_sp_pub(-1), _force_sp_pub(-1),
_pos_sp_triplet_pub(-1), _pos_sp_triplet_pub(-1),
_vicon_position_pub(-1), _vicon_position_pub(-1),
_vision_position_pub(-1), _vision_position_pub(-1),
_telemetry_status_pub(-1), _telemetry_status_pub(-1),
_rc_pub(-1), _rc_pub(-1),
_manual_pub(-1), _manual_pub(-1),
_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
_hil_frames(0), _hil_frames(0),
_old_timestamp(0), _old_timestamp(0),
_hil_local_proj_inited(0), _hil_local_proj_inited(0),
_hil_local_alt0(0.0f), _hil_local_alt0(0.0f),
_hil_local_proj_ref {}, _hil_local_proj_ref{}
_time_offset_avg_alpha(0.75),
_time_offset(0)
{ {
// make sure the FTP server is started // make sure the FTP server is started
@@ -232,7 +230,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
} }
if (_mavlink->get_hil_enabled() || (_mavlink->get_use_hil_gps() && msg->sysid == mavlink_system.sysid)) { if (_mavlink->get_hil_enabled() || (_mavlink->get_use_hil_gps() && msg->sysid == mavlink_system.sysid)) {
switch (msg->msgid) { switch (msg->msgid) {
case MAVLINK_MSG_ID_HIL_GPS: case MAVLINK_MSG_ID_HIL_GPS:
handle_message_hil_gps(msg); handle_message_hil_gps(msg);
@@ -244,7 +242,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
} }
/* If we've received a valid message, mark the flag indicating so. /* If we've received a valid message, mark the flag indicating so.
This is used in the '-w' command-line flag. */ This is used in the '-w' command-line flag. */
_mavlink->set_has_received_messages(true); _mavlink->set_has_received_messages(true);
} }
@@ -277,35 +275,22 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
} }
struct vehicle_command_s vcmd; struct vehicle_command_s vcmd;
memset(&vcmd, 0, sizeof(vcmd)); memset(&vcmd, 0, sizeof(vcmd));
/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
vcmd.param1 = cmd_mavlink.param1; vcmd.param1 = cmd_mavlink.param1;
vcmd.param2 = cmd_mavlink.param2; vcmd.param2 = cmd_mavlink.param2;
vcmd.param3 = cmd_mavlink.param3; vcmd.param3 = cmd_mavlink.param3;
vcmd.param4 = cmd_mavlink.param4; vcmd.param4 = cmd_mavlink.param4;
vcmd.param5 = cmd_mavlink.param5; vcmd.param5 = cmd_mavlink.param5;
vcmd.param6 = cmd_mavlink.param6; vcmd.param6 = cmd_mavlink.param6;
vcmd.param7 = cmd_mavlink.param7; vcmd.param7 = cmd_mavlink.param7;
// XXX do proper translation // XXX do proper translation
vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command; vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command;
vcmd.target_system = cmd_mavlink.target_system; vcmd.target_system = cmd_mavlink.target_system;
vcmd.target_component = cmd_mavlink.target_component; vcmd.target_component = cmd_mavlink.target_component;
vcmd.source_system = msg->sysid; vcmd.source_system = msg->sysid;
vcmd.source_component = msg->compid; vcmd.source_component = msg->compid;
vcmd.confirmation = cmd_mavlink.confirmation; vcmd.confirmation = cmd_mavlink.confirmation;
if (_cmd_pub < 0) { if (_cmd_pub < 0) {
@@ -346,34 +331,22 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
} }
struct vehicle_command_s vcmd; struct vehicle_command_s vcmd;
memset(&vcmd, 0, sizeof(vcmd)); memset(&vcmd, 0, sizeof(vcmd));
/* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */ /* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */
vcmd.param1 = cmd_mavlink.param1; vcmd.param1 = cmd_mavlink.param1;
vcmd.param2 = cmd_mavlink.param2; vcmd.param2 = cmd_mavlink.param2;
vcmd.param3 = cmd_mavlink.param3; vcmd.param3 = cmd_mavlink.param3;
vcmd.param4 = cmd_mavlink.param4; vcmd.param4 = cmd_mavlink.param4;
/* these are coordinates as 1e7 scaled integers to work around the 32 bit floating point limits */ /* these are coordinates as 1e7 scaled integers to work around the 32 bit floating point limits */
vcmd.param5 = ((double)cmd_mavlink.x) / 1e7; vcmd.param5 = ((double)cmd_mavlink.x) / 1e7;
vcmd.param6 = ((double)cmd_mavlink.y) / 1e7; vcmd.param6 = ((double)cmd_mavlink.y) / 1e7;
vcmd.param7 = cmd_mavlink.z; vcmd.param7 = cmd_mavlink.z;
// XXX do proper translation // XXX do proper translation
vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command; vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command;
vcmd.target_system = cmd_mavlink.target_system; vcmd.target_system = cmd_mavlink.target_system;
vcmd.target_component = cmd_mavlink.target_component; vcmd.target_component = cmd_mavlink.target_component;
vcmd.source_system = msg->sysid; vcmd.source_system = msg->sysid;
vcmd.source_component = msg->compid; vcmd.source_component = msg->compid;
if (_cmd_pub < 0) { if (_cmd_pub < 0) {
@@ -461,7 +434,6 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
if (_range_pub < 0) { if (_range_pub < 0) {
_range_pub = orb_advertise(ORB_ID(sensor_range_finder), &r); _range_pub = orb_advertise(ORB_ID(sensor_range_finder), &r);
} else { } else {
orb_publish(ORB_ID(sensor_range_finder), _range_pub, &r); orb_publish(ORB_ID(sensor_range_finder), _range_pub, &r);
} }
@@ -537,33 +509,28 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
/* Only accept messages which are intended for this system */ /* Only accept messages which are intended for this system */
if ((mavlink_system.sysid == set_position_target_local_ned.target_system || if ((mavlink_system.sysid == set_position_target_local_ned.target_system ||
set_position_target_local_ned.target_system == 0) && set_position_target_local_ned.target_system == 0) &&
(mavlink_system.compid == set_position_target_local_ned.target_component || (mavlink_system.compid == set_position_target_local_ned.target_component ||
set_position_target_local_ned.target_component == 0)) { set_position_target_local_ned.target_component == 0)) {
/* convert mavlink type (local, NED) to uORB offboard control struct */ /* convert mavlink type (local, NED) to uORB offboard control struct */
switch (set_position_target_local_ned.coordinate_frame) { switch (set_position_target_local_ned.coordinate_frame) {
case MAV_FRAME_LOCAL_NED: case MAV_FRAME_LOCAL_NED:
offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED; offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED;
break; break;
case MAV_FRAME_LOCAL_OFFSET_NED:
case MAV_FRAME_LOCAL_OFFSET_NED: offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED;
offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED; break;
break; case MAV_FRAME_BODY_NED:
offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED;
case MAV_FRAME_BODY_NED: break;
offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED; case MAV_FRAME_BODY_OFFSET_NED:
break; offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED;
break;
case MAV_FRAME_BODY_OFFSET_NED: default:
offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED; /* invalid setpoint, avoid publishing */
break; return;
default:
/* invalid setpoint, avoid publishing */
return;
} }
offboard_control_sp.position[0] = set_position_target_local_ned.x; 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[1] = set_position_target_local_ned.y;
offboard_control_sp.position[2] = set_position_target_local_ned.z; offboard_control_sp.position[2] = set_position_target_local_ned.z;
@@ -585,19 +552,17 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
/* set ignore flags */ /* set ignore flags */
for (int i = 0; i < 9; i++) { for (int i = 0; i < 9; i++) {
offboard_control_sp.ignore &= ~(1 << i); offboard_control_sp.ignore &= ~(1 << i);
offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (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 &= ~(1 << OFB_IGN_BIT_YAW);
if (set_position_target_local_ned.type_mask & (1 << 10)) { 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_YAW);
} }
offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAWRATE); offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAWRATE);
if (set_position_target_local_ned.type_mask & (1 << 11)) { if (set_position_target_local_ned.type_mask & (1 << 11)) {
offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAWRATE); offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAWRATE);
} }
offboard_control_sp.timestamp = hrt_absolute_time(); offboard_control_sp.timestamp = hrt_absolute_time();
@@ -614,30 +579,25 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
if (_mavlink->get_forward_externalsp()) { if (_mavlink->get_forward_externalsp()) {
bool updated; bool updated;
orb_check(_control_mode_sub, &updated); orb_check(_control_mode_sub, &updated);
if (updated) { if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
} }
if (_control_mode.flag_control_offboard_enabled) { if (_control_mode.flag_control_offboard_enabled) {
if (offboard_control_sp.isForceSetpoint && if (offboard_control_sp.isForceSetpoint &&
offboard_control_sp_ignore_position_all(offboard_control_sp) && offboard_control_sp_ignore_position_all(offboard_control_sp) &&
offboard_control_sp_ignore_velocity_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 /* The offboard setpoint is a force setpoint only, directly writing to the force
* setpoint topic and not publishing the setpoint triplet topic */ * setpoint topic and not publishing the setpoint triplet topic */
struct vehicle_force_setpoint_s force_sp; struct vehicle_force_setpoint_s force_sp;
force_sp.x = offboard_control_sp.acceleration[0]; force_sp.x = offboard_control_sp.acceleration[0];
force_sp.y = offboard_control_sp.acceleration[1]; force_sp.y = offboard_control_sp.acceleration[1];
force_sp.z = offboard_control_sp.acceleration[2]; force_sp.z = offboard_control_sp.acceleration[2];
//XXX: yaw //XXX: yaw
if (_force_sp_pub < 0) { if (_force_sp_pub < 0) {
_force_sp_pub = orb_advertise(ORB_ID(vehicle_force_setpoint), &force_sp); _force_sp_pub = orb_advertise(ORB_ID(vehicle_force_setpoint), &force_sp);
} else { } else {
orb_publish(ORB_ID(vehicle_force_setpoint), _force_sp_pub, &force_sp); orb_publish(ORB_ID(vehicle_force_setpoint), _force_sp_pub, &force_sp);
} }
} else { } else {
/* It's not a pure force setpoint: publish to setpoint triplet topic */ /* It's not a pure force setpoint: publish to setpoint triplet topic */
struct position_setpoint_triplet_s pos_sp_triplet; struct position_setpoint_triplet_s pos_sp_triplet;
@@ -649,12 +609,11 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
/* set the local pos values if the setpoint type is 'local pos' and none /* set the local pos values if the setpoint type is 'local pos' and none
* of the local pos fields is set to 'ignore' */ * of the local pos fields is set to 'ignore' */
if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
!offboard_control_sp_ignore_position_some(offboard_control_sp)) { !offboard_control_sp_ignore_position_some(offboard_control_sp)) {
pos_sp_triplet.current.position_valid = true; pos_sp_triplet.current.position_valid = true;
pos_sp_triplet.current.x = offboard_control_sp.position[0]; pos_sp_triplet.current.x = offboard_control_sp.position[0];
pos_sp_triplet.current.y = offboard_control_sp.position[1]; pos_sp_triplet.current.y = offboard_control_sp.position[1];
pos_sp_triplet.current.z = offboard_control_sp.position[2]; pos_sp_triplet.current.z = offboard_control_sp.position[2];
} else { } else {
pos_sp_triplet.current.position_valid = false; pos_sp_triplet.current.position_valid = false;
} }
@@ -662,12 +621,11 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
/* set the local vel values if the setpoint type is 'local pos' and none /* set the local vel values if the setpoint type is 'local pos' and none
* of the local vel fields is set to 'ignore' */ * of the local vel fields is set to 'ignore' */
if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
!offboard_control_sp_ignore_velocity_some(offboard_control_sp)) { !offboard_control_sp_ignore_velocity_some(offboard_control_sp)) {
pos_sp_triplet.current.velocity_valid = true; pos_sp_triplet.current.velocity_valid = true;
pos_sp_triplet.current.vx = offboard_control_sp.velocity[0]; pos_sp_triplet.current.vx = offboard_control_sp.velocity[0];
pos_sp_triplet.current.vy = offboard_control_sp.velocity[1]; pos_sp_triplet.current.vy = offboard_control_sp.velocity[1];
pos_sp_triplet.current.vz = offboard_control_sp.velocity[2]; pos_sp_triplet.current.vz = offboard_control_sp.velocity[2];
} else { } else {
pos_sp_triplet.current.velocity_valid = false; pos_sp_triplet.current.velocity_valid = false;
} }
@@ -675,13 +633,13 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
/* set the local acceleration values if the setpoint type is 'local pos' and none /* set the local acceleration values if the setpoint type is 'local pos' and none
* of the accelerations fields is set to 'ignore' */ * of the accelerations fields is set to 'ignore' */
if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
!offboard_control_sp_ignore_acceleration_some(offboard_control_sp)) { !offboard_control_sp_ignore_acceleration_some(offboard_control_sp)) {
pos_sp_triplet.current.acceleration_valid = true; pos_sp_triplet.current.acceleration_valid = true;
pos_sp_triplet.current.a_x = offboard_control_sp.acceleration[0]; 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_y = offboard_control_sp.acceleration[1];
pos_sp_triplet.current.a_z = offboard_control_sp.acceleration[2]; pos_sp_triplet.current.a_z = offboard_control_sp.acceleration[2];
pos_sp_triplet.current.acceleration_is_force = pos_sp_triplet.current.acceleration_is_force =
offboard_control_sp.isForceSetpoint; offboard_control_sp.isForceSetpoint;
} else { } else {
pos_sp_triplet.current.acceleration_valid = false; pos_sp_triplet.current.acceleration_valid = false;
@@ -690,7 +648,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
/* set the yaw sp value if the setpoint type is 'local pos' and the yaw /* set the yaw sp value if the setpoint type is 'local pos' and the yaw
* field is not set to 'ignore' */ * field is not set to 'ignore' */
if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
!offboard_control_sp_ignore_yaw(offboard_control_sp)) { !offboard_control_sp_ignore_yaw(offboard_control_sp)) {
pos_sp_triplet.current.yaw_valid = true; pos_sp_triplet.current.yaw_valid = true;
pos_sp_triplet.current.yaw = offboard_control_sp.yaw; pos_sp_triplet.current.yaw = offboard_control_sp.yaw;
@@ -701,24 +659,22 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
/* set the yawrate sp value if the setpoint type is 'local pos' and the yawrate /* set the yawrate sp value if the setpoint type is 'local pos' and the yawrate
* field is not set to 'ignore' */ * field is not set to 'ignore' */
if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
!offboard_control_sp_ignore_yawrate(offboard_control_sp)) { !offboard_control_sp_ignore_yawrate(offboard_control_sp)) {
pos_sp_triplet.current.yawspeed_valid = true; pos_sp_triplet.current.yawspeed_valid = true;
pos_sp_triplet.current.yawspeed = offboard_control_sp.yaw_rate; pos_sp_triplet.current.yawspeed = offboard_control_sp.yaw_rate;
} else { } else {
pos_sp_triplet.current.yawspeed_valid = false; pos_sp_triplet.current.yawspeed_valid = false;
} }
//XXX handle global pos setpoints (different MAV frames) //XXX handle global pos setpoints (different MAV frames)
if (_pos_sp_triplet_pub < 0) { if (_pos_sp_triplet_pub < 0) {
_pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet),
&pos_sp_triplet); &pos_sp_triplet);
} else { } else {
orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub,
&pos_sp_triplet); &pos_sp_triplet);
} }
} }
@@ -779,13 +735,12 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
/* Only accept messages which are intended for this system */ /* Only accept messages which are intended for this system */
if ((mavlink_system.sysid == set_attitude_target.target_system || if ((mavlink_system.sysid == set_attitude_target.target_system ||
set_attitude_target.target_system == 0) && set_attitude_target.target_system == 0) &&
(mavlink_system.compid == set_attitude_target.target_component || (mavlink_system.compid == set_attitude_target.target_component ||
set_attitude_target.target_component == 0)) { set_attitude_target.target_component == 0)) {
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
offboard_control_sp.attitude[i] = set_attitude_target.q[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[0] = set_attitude_target.body_roll_rate;
offboard_control_sp.attitude_rate[1] = set_attitude_target.body_pitch_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; offboard_control_sp.attitude_rate[2] = set_attitude_target.body_yaw_rate;
@@ -793,9 +748,8 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
/* set correct ignore flags for body rate fields: copy from mavlink message */ /* set correct ignore flags for body rate fields: copy from mavlink message */
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
offboard_control_sp.ignore &= ~(1 << (i + OFB_IGN_BIT_BODYRATE_X)); 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_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 */ /* set correct ignore flags for thrust field: copy from mavlink message */
offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_THRUST); 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_sp.ignore |= ((set_attitude_target.type_mask & (1 << 6)) << OFB_IGN_BIT_THRUST);
@@ -805,7 +759,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
offboard_control_sp.timestamp = hrt_absolute_time(); offboard_control_sp.timestamp = hrt_absolute_time();
offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; //XXX handle rate control mode offboard_control_sp.mode =OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; //XXX handle rate control mode
if (_offboard_control_sp_pub < 0) { if (_offboard_control_sp_pub < 0) {
_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
@@ -819,7 +773,6 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
if (_mavlink->get_forward_externalsp()) { if (_mavlink->get_forward_externalsp()) {
bool updated; bool updated;
orb_check(_control_mode_sub, &updated); orb_check(_control_mode_sub, &updated);
if (updated) { if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
} }
@@ -828,20 +781,18 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
/* Publish attitude setpoint if attitude and thrust ignore bits are not set */ /* Publish attitude setpoint if attitude and thrust ignore bits are not set */
if (!(offboard_control_sp_ignore_attitude(offboard_control_sp) || if (!(offboard_control_sp_ignore_attitude(offboard_control_sp) ||
offboard_control_sp_ignore_thrust(offboard_control_sp))) { offboard_control_sp_ignore_thrust(offboard_control_sp))) {
struct vehicle_attitude_setpoint_s att_sp; struct vehicle_attitude_setpoint_s att_sp;
att_sp.timestamp = hrt_absolute_time(); att_sp.timestamp = hrt_absolute_time();
mavlink_quaternion_to_euler(set_attitude_target.q, mavlink_quaternion_to_euler(set_attitude_target.q,
&att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body);
mavlink_quaternion_to_dcm(set_attitude_target.q, att_sp.R_body); mavlink_quaternion_to_dcm(set_attitude_target.q, att_sp.R_body);
att_sp.R_valid = true; att_sp.R_valid = true;
att_sp.thrust = set_attitude_target.thrust; att_sp.thrust = set_attitude_target.thrust;
memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d)); memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d));
att_sp.q_d_valid = true; att_sp.q_d_valid = true;
if (_att_sp_pub < 0) { if (_att_sp_pub < 0) {
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
} else { } else {
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp); orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp);
} }
@@ -850,7 +801,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
/* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */ /* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */
///XXX add support for ignoring individual axes ///XXX add support for ignoring individual axes
if (!(offboard_control_sp_ignore_bodyrates_some(offboard_control_sp) || if (!(offboard_control_sp_ignore_bodyrates_some(offboard_control_sp) ||
offboard_control_sp_ignore_thrust(offboard_control_sp))) { offboard_control_sp_ignore_thrust(offboard_control_sp))) {
struct vehicle_rates_setpoint_s rates_sp; struct vehicle_rates_setpoint_s rates_sp;
rates_sp.timestamp = hrt_absolute_time(); rates_sp.timestamp = hrt_absolute_time();
rates_sp.roll = set_attitude_target.body_roll_rate; rates_sp.roll = set_attitude_target.body_roll_rate;
@@ -860,7 +811,6 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
if (_att_sp_pub < 0) { if (_att_sp_pub < 0) {
_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
} else { } else {
orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp); orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp);
} }
@@ -919,8 +869,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
manual.r = man.r / 1000.0f; manual.r = man.r / 1000.0f;
manual.z = man.z / 1000.0f; manual.z = man.z / 1000.0f;
warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, (double)manual.z);
(double)manual.z);
if (_manual_pub < 0) { if (_manual_pub < 0) {
_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);