From ce6fb7b77cba3dbb70a9fca91d85f823cf6c523b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 10 Aug 2014 19:07:26 +0200 Subject: [PATCH] Updated MAVLink to most recent revision, implemented all changed functionality equivalent --- libs/GLC_lib | 1 - src/comm/MAVLinkSimulationLink.cc | 24 ---------- src/comm/MAVLinkSimulationMAV.cc | 23 ---------- src/comm/MAVLinkSimulationWaypointPlanner.cc | 28 +----------- src/uas/UAS.cc | 46 ++++++++++++++------ 5 files changed, 33 insertions(+), 89 deletions(-) delete mode 160000 libs/GLC_lib diff --git a/libs/GLC_lib b/libs/GLC_lib deleted file mode 160000 index ef1adb084..000000000 --- a/libs/GLC_lib +++ /dev/null @@ -1 +0,0 @@ -Subproject commit ef1adb0843a9f8073bce9e641b2a0d9bf002ea39 diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index 1703c2342..248f7c84d 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -394,13 +394,7 @@ void MAVLinkSimulationLink::mainloop() // y = (y < -5.0f) ? -5.0f : y; // z = (z < -3.0f) ? -3.0f : z; - // Send back new setpoint mavlink_message_t ret; - mavlink_msg_local_position_setpoint_pack(systemId, componentId, &ret, MAV_FRAME_LOCAL_NED, spX, spY, spZ, spYaw); // spYaw/180.0*M_PI); - bufferlength = mavlink_msg_to_send_buffer(buffer, &ret); - //add data into datastream - memcpy(stream+streampointer,buffer, bufferlength); - streampointer += bufferlength; // Send back new position mavlink_msg_local_position_ned_pack(systemId, componentId, &ret, 0, x, y, -fabs(z), xSpeed, ySpeed, zSpeed); @@ -702,24 +696,6 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) system.base_mode = mode.base_mode; } break; - case MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT: - { - mavlink_set_local_position_setpoint_t set; - mavlink_msg_set_local_position_setpoint_decode(&msg, &set); - spX = set.x; - spY = set.y; - spZ = set.z; - spYaw = set.yaw; - - // Send back new setpoint - mavlink_message_t ret; - mavlink_msg_local_position_setpoint_pack(systemId, componentId, &ret, MAV_FRAME_LOCAL_NED, spX, spY, spZ, spYaw); - bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); - //add data into datastream - memcpy(stream+streampointer,buffer, bufferlength); - streampointer += bufferlength; - } - break; // EXECUTE OPERATOR ACTIONS case MAVLINK_MSG_ID_COMMAND_LONG: { diff --git a/src/comm/MAVLinkSimulationMAV.cc b/src/comm/MAVLinkSimulationMAV.cc index 51e2669b8..e9c462483 100644 --- a/src/comm/MAVLinkSimulationMAV.cc +++ b/src/comm/MAVLinkSimulationMAV.cc @@ -518,28 +518,5 @@ void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg) // } // } break; - case MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT: { - mavlink_set_local_position_setpoint_t sp; - mavlink_msg_set_local_position_setpoint_decode(&msg, &sp); - if (sp.target_system == this->systemid) { - nav_mode = 0; - previousSPX = nextSPX; - previousSPY = nextSPY; - previousSPZ = nextSPZ; - nextSPX = sp.x; - nextSPY = sp.y; - nextSPZ = sp.z; - - // Rotary wing - //nextSPYaw = sp.yaw; - - // Airplane - //yaw = atan2(previousSPX-nextSPX, previousSPY-nextSPY); - - //if (!firstWP) firstWP = true; - } - //qDebug() << "UPDATED SP:" << "X" << nextSPX << "Y" << nextSPY << "Z" << nextSPZ; - } - break; } } diff --git a/src/comm/MAVLinkSimulationWaypointPlanner.cc b/src/comm/MAVLinkSimulationWaypointPlanner.cc index 448f49937..c4cc5fee3 100644 --- a/src/comm/MAVLinkSimulationWaypointPlanner.cc +++ b/src/comm/MAVLinkSimulationWaypointPlanner.cc @@ -522,33 +522,7 @@ void MAVLinkSimulationWaypointPlanner::send_waypoint_current(uint16_t seq) */ void MAVLinkSimulationWaypointPlanner::send_setpoint(uint16_t seq) { - if(seq < waypoints->size()) { - mavlink_mission_item_t *cur = waypoints->at(seq); - - // send new set point to local IMU - if (cur->frame == MAV_FRAME_LOCAL_NED || cur->frame == MAV_FRAME_LOCAL_ENU) { - mavlink_message_t msg; - mavlink_set_local_position_setpoint_t PControlSetPoint; - - PControlSetPoint.target_system = systemid; - PControlSetPoint.target_component = MAV_COMP_ID_IMU; - PControlSetPoint.x = cur->x; - PControlSetPoint.y = cur->y; - PControlSetPoint.z = cur->z; - PControlSetPoint.yaw = cur->param4; - PControlSetPoint.coordinate_frame = cur->frame; - - mavlink_msg_set_local_position_setpoint_encode(systemid, compid, &msg, &PControlSetPoint); - link->sendMAVLinkMessage(&msg); - emit messageSent(msg); - - uint64_t now = QGC::groundTimeMilliseconds(); - timestamp_last_send_setpoint = now; - } else if (verbose) { - qDebug("No new set point sent to IMU because the new waypoint %u had no local coordinates\n", cur->seq); - } - - } + Q_UNUSED(seq); } void MAVLinkSimulationWaypointPlanner::send_waypoint_count(uint8_t target_systemid, uint8_t target_compid, uint16_t count) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 83b1f6bf3..092f004c6 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -1123,12 +1123,14 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) break; } } - case MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT: + case MAVLINK_MSG_ID_ATTITUDE_TARGET: { - mavlink_roll_pitch_yaw_thrust_setpoint_t out; - mavlink_msg_roll_pitch_yaw_thrust_setpoint_decode(&message, &out); + mavlink_attitude_target_t out; + mavlink_msg_attitude_target_decode(&message, &out); + float roll, pitch, yaw; + mavlink_quaternion_to_euler(out.q, &roll, &pitch, &yaw); quint64 time = getUnixTimeFromMs(out.time_boot_ms); - emit attitudeThrustSetPointChanged(this, out.roll, out.pitch, out.yaw, out.thrust, time); + emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, out.thrust, time); } break; case MAVLINK_MSG_ID_MISSION_COUNT: @@ -1274,22 +1276,23 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) } break; - case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT: + case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED: { if (multiComponentSourceDetected && wrongComponent) { break; } - mavlink_local_position_setpoint_t p; - mavlink_msg_local_position_setpoint_decode(&message, &p); - emit positionSetPointsChanged(uasId, p.x, p.y, p.z, p.yaw, QGC::groundTimeUsecs()); + mavlink_position_target_local_ned_t p; + mavlink_msg_position_target_local_ned_decode(&message, &p); + quint64 time = getUnixTimeFromMs(p.time_boot_ms); + emit positionSetPointsChanged(uasId, p.x, p.y, p.z, 0/* XXX remove yaw and move it to attitude */, time); } break; - case MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT: + case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: { - mavlink_set_local_position_setpoint_t p; - mavlink_msg_set_local_position_setpoint_decode(&message, &p); - emit userPositionSetPointsChanged(uasId, p.x, p.y, p.z, p.yaw); + mavlink_set_position_target_local_ned_t p; + mavlink_msg_set_position_target_local_ned_decode(&message, &p); + emit userPositionSetPointsChanged(uasId, p.x, p.y, p.z, 0/* XXX remove yaw and move it to attitude */); } break; case MAVLINK_MSG_ID_STATUSTEXT: @@ -2905,9 +2908,24 @@ void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll if(((base_mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (base_mode & MAV_MODE_FLAG_HIL_ENABLED)) { mavlink_message_t message; - mavlink_msg_setpoint_6dof_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, (float)x, (float)y, (float)z, (float)roll, (float)pitch, (float)yaw); + float q[4]; + mavlink_euler_to_quaternion(roll, pitch, yaw, q); + quint8 mask; + // Do not control rates and throttle + mask |= (1 << 0) | (1 << 1) | (1 << 2); // ignore rates + mask |= (1 << 6); // ignore throttle + mavlink_msg_set_attitude_target_pack(mavlink->getSystemId(), mavlink->getComponentId(), + &message, QGC::groundTimeMilliseconds(), this->uasId, 0, + mask, q, 0, 0, 0, 0); + sendMessage(message); + quint8 position_mask; + position_mask |= (1 << 3) | (1 << 4) | (1 << 5) | + (1 << 6) | (1 << 7) | (1 << 8); + mavlink_msg_set_position_target_local_ned_pack(mavlink->getSystemId(), mavlink->getComponentId(), + &message, QGC::groundTimeMilliseconds(), this->uasId, 0, + MAV_FRAME_LOCAL_NED, position_mask, x, y, z, 0, 0, 0, 0, 0, 0); sendMessage(message); - qDebug() << __FILE__ << __LINE__ << ": SENT 6DOF CONTROL MESSAGE: x" << x << " y: " << y << " z: " << z << " roll: " << roll << " pitch: " << pitch << " yaw: " << yaw; + qDebug() << __FILE__ << __LINE__ << ": SENT 6DOF CONTROL MESSAGES: x" << x << " y: " << y << " z: " << z << " roll: " << roll << " pitch: " << pitch << " yaw: " << yaw; //emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds()); } -- 2.22.0