From 01776370526fe6bd665b4cacaec43d138c9ee041 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 10 Aug 2014 12:14:14 +0200 Subject: [PATCH 1/4] Updated MAVLink revision --- libs/GLC_lib | 1 + libs/mavlink/include/mavlink/v1.0 | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) create mode 160000 libs/GLC_lib diff --git a/libs/GLC_lib b/libs/GLC_lib new file mode 160000 index 0000000..ef1adb0 --- /dev/null +++ b/libs/GLC_lib @@ -0,0 +1 @@ +Subproject commit ef1adb0843a9f8073bce9e641b2a0d9bf002ea39 diff --git a/libs/mavlink/include/mavlink/v1.0 b/libs/mavlink/include/mavlink/v1.0 index 613e569..8cc2147 160000 --- a/libs/mavlink/include/mavlink/v1.0 +++ b/libs/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 613e56970458ff9003b966c5c6296a1d2eb05d2c +Subproject commit 8cc21472cce09709d4c651e2855d891843a17e41 From ce6fb7b77cba3dbb70a9fca91d85f823cf6c523b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 10 Aug 2014 19:07:26 +0200 Subject: [PATCH 2/4] 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 ef1adb0..0000000 --- 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 1703c23..248f7c8 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 51e2669..e9c4624 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 448f499..c4cc5fe 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 83b1f6b..092f004 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()); } From 6d28eda25a0fad3ae181e110ca8c7346cf4ec0e8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 10 Aug 2014 20:24:34 +0200 Subject: [PATCH 3/4] Removed unmaintained dialect messages --- src/uas/UAS.cc | 122 --------------------------------------------------------- 1 file changed, 122 deletions(-) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 092f004..6133baf 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -1384,126 +1384,16 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) imagePackets = 0; imagePacketsArrived = 0; emit imageReady(this); - //qDebug() << "imageReady emitted. all packets arrived"; } } break; - - // case MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT: - // { - // mavlink_object_detection_event_t event; - // mavlink_msg_object_detection_event_decode(&message, &event); - // QString str(event.name); - // emit objectDetected(event.time, event.object_id, event.type, str, event.quality, event.bearing, event.distance); - // } - // break; - // WILL BE ENABLED ONCE MESSAGE IS IN COMMON MESSAGE SET - // case MAVLINK_MSG_ID_MEMORY_VECT: - // { - // mavlink_memory_vect_t vect; - // mavlink_msg_memory_vect_decode(&message, &vect); - // QString str("mem_%1"); - // quint64 time = getUnixTime(0); - // int16_t *mem0 = (int16_t *)&vect.value[0]; - // uint16_t *mem1 = (uint16_t *)&vect.value[0]; - // int32_t *mem2 = (int32_t *)&vect.value[0]; - // // uint32_t *mem3 = (uint32_t *)&vect.value[0]; causes overload problem - // float *mem4 = (float *)&vect.value[0]; - // if ( vect.ver == 0) vect.type = 0, vect.ver = 1; else ; - // if ( vect.ver == 1) - // { - // switch (vect.type) { - // default: - // case 0: - // for (int i = 0; i < 16; i++) - // // FIXME REMOVE LATER emit valueChanged(uasId, str.arg(vect.address+(i*2)), "i16", mem0[i], time); - // break; - // case 1: - // for (int i = 0; i < 16; i++) - // // FIXME REMOVE LATER emit valueChanged(uasId, str.arg(vect.address+(i*2)), "ui16", mem1[i], time); - // break; - // case 2: - // for (int i = 0; i < 16; i++) - // // FIXME REMOVE LATER emit valueChanged(uasId, str.arg(vect.address+(i*2)), "Q15", (float)mem0[i]/32767.0, time); - // break; - // case 3: - // for (int i = 0; i < 16; i++) - // // FIXME REMOVE LATER emit valueChanged(uasId, str.arg(vect.address+(i*2)), "1Q14", (float)mem0[i]/16383.0, time); - // break; - // case 4: - // for (int i = 0; i < 8; i++) - // // FIXME REMOVE LATER emit valueChanged(uasId, str.arg(vect.address+(i*4)), "i32", mem2[i], time); - // break; - // case 5: - // for (int i = 0; i < 8; i++) - // // FIXME REMOVE LATER emit valueChanged(uasId, str.arg(vect.address+(i*4)), "i32", mem2[i], time); - // break; - // case 6: - // for (int i = 0; i < 8; i++) - // // FIXME REMOVE LATER emit valueChanged(uasId, str.arg(vect.address+(i*4)), "float", mem4[i], time); - // break; - // } - // } - // } - // break; -#ifdef MAVLINK_ENABLED_UALBERTA - case MAVLINK_MSG_ID_NAV_FILTER_BIAS: - { - mavlink_nav_filter_bias_t bias; - mavlink_msg_nav_filter_bias_decode(&message, &bias); - quint64 time = getUnixTime(); - // FIXME REMOVE LATER emit valueChanged(uasId, "b_f[0]", "raw", bias.accel_0, time); - // FIXME REMOVE LATER emit valueChanged(uasId, "b_f[1]", "raw", bias.accel_1, time); - // FIXME REMOVE LATER emit valueChanged(uasId, "b_f[2]", "raw", bias.accel_2, time); - // FIXME REMOVE LATER emit valueChanged(uasId, "b_w[0]", "raw", bias.gyro_0, time); - // FIXME REMOVE LATER emit valueChanged(uasId, "b_w[1]", "raw", bias.gyro_1, time); - // FIXME REMOVE LATER emit valueChanged(uasId, "b_w[2]", "raw", bias.gyro_2, time); - } - break; - case MAVLINK_MSG_ID_RADIO_CALIBRATION: - { - mavlink_radio_calibration_t radioMsg; - mavlink_msg_radio_calibration_decode(&message, &radioMsg); - QVector aileron; - QVector elevator; - QVector rudder; - QVector gyro; - QVector pitch; - QVector throttle; - - for (int i=0; i radioData = new RadioCalibrationData(aileron, elevator, rudder, gyro, pitch, throttle); - emit radioCalibrationReceived(radioData); - delete radioData; - } - break; - -#endif case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: { - //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_nav_controller_output_t p; mavlink_msg_nav_controller_output_decode(&message,&p); setDistToWaypoint(p.wp_dist); setBearingToWaypoint(p.nav_bearing); - //setAltitudeError(p.alt_error); - //setSpeedError(p.aspd_error); - //setCrosstrackingError(p.xtrack_error); emit navigationControllerErrorsChanged(this, p.alt_error, p.aspd_error, p.xtrack_error); } break; @@ -1618,16 +1508,10 @@ void UAS::setLocalOriginAtCurrentGPSPosition() */ void UAS::setLocalPositionSetpoint(float x, float y, float z, float yaw) { -#ifdef MAVLINK_ENABLED_PIXHAWK - mavlink_message_t msg; - mavlink_msg_set_local_position_setpoint_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_FRAME_LOCAL_NED, x, y, z, yaw/M_PI*180.0); - sendMessage(msg); -#else Q_UNUSED(x); Q_UNUSED(y); Q_UNUSED(z); Q_UNUSED(yaw); -#endif } /** @@ -1639,16 +1523,10 @@ void UAS::setLocalPositionSetpoint(float x, float y, float z, float yaw) */ void UAS::setLocalPositionOffset(float x, float y, float z, float yaw) { -#ifdef MAVLINK_ENABLED_PIXHAWK - mavlink_message_t msg; - mavlink_msg_set_position_control_offset_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, x, y, z, yaw); - sendMessage(msg); -#else Q_UNUSED(x); Q_UNUSED(y); Q_UNUSED(z); Q_UNUSED(yaw); -#endif } void UAS::startRadioControlCalibration(int param) From 652e7b43a275cf338b41b219e219ac4a34c14495 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 10 Aug 2014 22:01:40 +0200 Subject: [PATCH 4/4] Updated MAVLink message specs, now got FILE_TRANSFER_PROTOCOL --- libs/mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libs/mavlink/include/mavlink/v1.0 b/libs/mavlink/include/mavlink/v1.0 index 8cc2147..4cd3840 160000 --- a/libs/mavlink/include/mavlink/v1.0 +++ b/libs/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 8cc21472cce09709d4c651e2855d891843a17e41 +Subproject commit 4cd384001b5373e1ecaa6c4cd66994855cec4742