zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Updated MAVLink to most recent revision, implemented all changed functionality equivalent

QGC4.4
Lorenz Meier 11 years ago
parent
0177637052
commit
ce6fb7b77c
5 changed files with 33 additions and 89 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1
      libs/GLC_lib
  2. 24
      src/comm/MAVLinkSimulationLink.cc
  3. 23
      src/comm/MAVLinkSimulationMAV.cc
  4. 28
      src/comm/MAVLinkSimulationWaypointPlanner.cc
  5. 46
      src/uas/UAS.cc

1
libs/GLC_lib

@ -1 +0,0 @@ @@ -1 +0,0 @@
Subproject commit ef1adb0843a9f8073bce9e641b2a0d9bf002ea39

24
src/comm/MAVLinkSimulationLink.cc
Unescape View File

@ -394,13 +394,7 @@ void MAVLinkSimulationLink::mainloop() @@ -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) @@ -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:
{

23
src/comm/MAVLinkSimulationMAV.cc
Unescape View File

@ -518,28 +518,5 @@ void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg) @@ -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;
}
}

28
src/comm/MAVLinkSimulationWaypointPlanner.cc
Unescape View File

@ -522,33 +522,7 @@ void MAVLinkSimulationWaypointPlanner::send_waypoint_current(uint16_t seq) @@ -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)

46
src/uas/UAS.cc
Unescape View File

@ -1123,12 +1123,14 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -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) @@ -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 @@ -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());
}

Write Preview
Loading…
Cancel
Save