diff options
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 88 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.h | 1 | ||||
-rw-r--r-- | src/modules/uORB/topics/offboard_control_setpoint.h | 6 |
3 files changed, 93 insertions, 2 deletions
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 9df843e58..9e3842614 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -157,6 +157,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_local_ned_position_setpoint_external(msg); break; + case MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL: + handle_message_attitude_setpoint_external(msg); + break; + case MAVLINK_MSG_ID_RADIO_STATUS: handle_message_radio_status(msg); break; @@ -404,7 +408,7 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes mavlink_msg_local_ned_position_setpoint_external_decode(msg, &local_ned_position_setpoint_external); struct offboard_control_setpoint_s offboard_control_sp; - memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); + memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));//XXX breaks compatibility with multiple setpoints if (mavlink_system.sysid == local_ned_position_setpoint_external.target_system && mavlink_system.compid == local_ned_position_setpoint_external.target_component) { @@ -438,7 +442,7 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes 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)); for (int i = 0; i < 9; i++) { - offboard_control_sp.ignore &= ~(local_ned_position_setpoint_external.type_mask & (1 << i)); + offboard_control_sp.ignore &= ~(1 << i); offboard_control_sp.ignore |= (local_ned_position_setpoint_external.type_mask & (1 << i)); } @@ -470,6 +474,86 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes } void +MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *msg) +{ + mavlink_attitude_setpoint_external_t attitude_setpoint_external; + mavlink_msg_attitude_setpoint_external_decode(msg, &attitude_setpoint_external); + + struct offboard_control_setpoint_s offboard_control_sp; + memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); //XXX breaks compatibility with multiple setpoints + + if (mavlink_system.sysid == attitude_setpoint_external.target_system && + mavlink_system.compid == attitude_setpoint_external.target_component) { + for (int i = 0; i < 4; i++) { + offboard_control_sp.attitude[i] = attitude_setpoint_external.q[i]; + } + offboard_control_sp.attitude_rate[0] = attitude_setpoint_external.body_roll_rate; + offboard_control_sp.attitude_rate[1] = attitude_setpoint_external.body_pitch_rate; + offboard_control_sp.attitude_rate[2] = attitude_setpoint_external.body_yaw_rate; + + for (int i = 0; i < 3; i++) { + offboard_control_sp.ignore &= ~(1 << (i + 9)); + offboard_control_sp.ignore |= (attitude_setpoint_external.type_mask & (1 << i)) << 9; + } + offboard_control_sp.ignore &= ~(1 << 10); + offboard_control_sp.ignore |= ((attitude_setpoint_external.type_mask & (1 << 7)) << 3); + + + 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) { + + /* Publish attitude setpoint if ignore bit is not set */ + if (!(attitude_setpoint_external.type_mask & (1 << 7))) { + struct vehicle_attitude_setpoint_s att_sp; + mavlink_quaternion_to_euler(attitude_setpoint_external.q, + &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); + att_sp.thrust = attitude_setpoint_external.thrust; + 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 ignore bit are not set */ + ///XXX add support for ignoring individual axes + if (!(attitude_setpoint_external.type_mask & (0b111))) { + struct vehicle_rates_setpoint_s rates_sp; + rates_sp.roll = attitude_setpoint_external.body_roll_rate; + rates_sp.pitch = attitude_setpoint_external.body_pitch_rate; + rates_sp.yaw = attitude_setpoint_external.body_yaw_rate; + rates_sp.thrust = attitude_setpoint_external.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) { /* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */ diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 4fd80f4ac..0f12cf32c 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -114,6 +114,7 @@ private: void handle_message_vicon_position_estimate(mavlink_message_t *msg); void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg); void handle_message_local_ned_position_setpoint_external(mavlink_message_t *msg); + void handle_message_attitude_setpoint_external(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); diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 809412fc0..809a2ace4 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -88,6 +88,7 @@ struct offboard_control_setpoint_s { float attitude_rate[3]; /**< body angular rates (x, y, z) */ uint16_t ignore; /**< if field i is set to true, pi should be ignored */ + //XXX define constants for bit offsets bool isForceSetpoint; /**< the acceleration vector should be interpreted as force */ float override_mode_switch; @@ -118,6 +119,11 @@ inline bool offboard_control_sp_ignore_bodyrates(const struct offboard_control_s return (bool)(offboard_control_sp.ignore & (1 << (9 + index))); } +inline bool offboard_control_sp_ignore_attitude(const struct offboard_control_setpoint_s &offboard_control_sp) { + return (bool)(offboard_control_sp.ignore & (1 << 10)); +} + + /* register this as object request broker structure */ ORB_DECLARE(offboard_control_setpoint); |