Skip to content

Q
qgroundcontrol
Commit ce6fb7b7 authored by Lorenz Meier's avatar Lorenz Meier
Browse files
Options

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

parent 01776370
Show whitespace changes
Inline Side-by-side
Showing with 33 additions and 89 deletions
GLC_lib @ ef1adb08
Subproject commit ef1adb0843a9f8073bce9e641b2a0d9bf002ea39
src/comm/MAVLinkSimulationLink.cc
View file @ ce6fb7b7
......@@ -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:
{
......
src/comm/MAVLinkSimulationMAV.cc
View file @ ce6fb7b7
......@@ -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;
}
}
src/comm/MAVLinkSimulationWaypointPlanner.cc
View file @ ce6fb7b7
......@@ -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)
......
src/uas/UAS.cc
View file @ ce6fb7b7
......@@ -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());
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment