Skip to content

Q
qgroundcontrol
Commit 161096d0 authored by DonLakeFlyer's avatar DonLakeFlyer
Browse files
Options

Move mavlink attitude handling to Vehicle

parent 11494b72
Hide whitespace changes
Inline Side-by-side
Showing with 31 additions and 146 deletions
src/Vehicle/Vehicle.cc
View file @ 161096d0
......@@ -235,9 +235,6 @@ Vehicle::Vehicle(LinkInterface* link,
// Listen for system messages
connect(_toolbox->uasMessageHandler(), &UASMessageHandler::textMessageCountChanged, this, &Vehicle::_handleTextMessage);
connect(_toolbox->uasMessageHandler(), &UASMessageHandler::textMessageReceived, this, &Vehicle::_handletextMessageReceived);
// Now connect the new UAS
connect(_mav, SIGNAL(attitudeChanged (UASInterface*,double,double,double,quint64)), this, SLOT(_updateAttitude(UASInterface*, double, double, double, quint64)));
connect(_mav, SIGNAL(attitudeChanged (UASInterface*,int,double,double,double,quint64)), this, SLOT(_updateAttitude(UASInterface*,int,double, double, double, quint64)));
if (_highLatencyLink || link->isPX4Flow()) {
// These links don't request information
......@@ -709,8 +706,8 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
case MAVLINK_MSG_ID_HIGH_LATENCY2:
_handleHighLatency2(message);
break;
case MAVLINK_MSG_ID_ATTITUDE:
_handleAttitude(message);
case MAVLINK_MSG_ID_ATTITUDE_QUATERNION:
_handleAttitudeQuaternion(message);
break;
case MAVLINK_MSG_ID_ATTITUDE_TARGET:
_handleAttitudeTarget(message);
......@@ -846,15 +843,37 @@ void Vehicle::_handleAttitudeTarget(mavlink_message_t& message)
_setpointFactGroup.yawRate()->setRawValue(qRadiansToDegrees(attitudeTarget.body_yaw_rate));
}
void Vehicle::_handleAttitude(mavlink_message_t& message)
void Vehicle::_handleAttitudeQuaternion(mavlink_message_t& message)
{
mavlink_attitude_t attitude;
mavlink_attitude_quaternion_t attitudeQuaternion;
mavlink_msg_attitude_decode(&message, &attitude);
mavlink_msg_attitude_quaternion_decode(&message, &attitudeQuaternion);
rollRate()->setRawValue(qRadiansToDegrees(attitude.rollspeed));
pitchRate()->setRawValue(qRadiansToDegrees(attitude.pitchspeed));
yawRate()->setRawValue(qRadiansToDegrees(attitude.yawspeed));
float roll, pitch, yaw;
float q[] = { attitudeQuaternion.q1, attitudeQuaternion.q2, attitudeQuaternion.q3, attitudeQuaternion.q4 };
mavlink_quaternion_to_euler(q, &roll, &pitch, &yaw);
roll = QGC::limitAngleToPMPIf(roll);
pitch = QGC::limitAngleToPMPIf(pitch);
yaw = QGC::limitAngleToPMPIf(yaw);
roll = qRadiansToDegrees(roll);
pitch = qRadiansToDegrees(pitch);
yaw = qRadiansToDegrees(yaw);
if (yaw < 0.0) {
yaw += 360.0;
}
// truncate to integer so widget never displays 360
yaw = trunc(yaw);
_rollFact.setRawValue(roll);
_pitchFact.setRawValue(pitch);
_headingFact.setRawValue(yaw);
rollRate()->setRawValue(qRadiansToDegrees(attitudeQuaternion.rollspeed));
pitchRate()->setRawValue(qRadiansToDegrees(attitudeQuaternion.pitchspeed));
yawRate()->setRawValue(qRadiansToDegrees(attitudeQuaternion.yawspeed));
}
void Vehicle::_handleGpsRawInt(mavlink_message_t& message)
......@@ -1788,33 +1807,6 @@ void Vehicle::_updatePriorityLink(bool updateActive, bool sendCommand)
}
}
void Vehicle::_updateAttitude(UASInterface*, double roll, double pitch, double yaw, quint64)
{
if (qIsInf(roll)) {
_rollFact.setRawValue(0);
} else {
_rollFact.setRawValue(roll * (180.0 / M_PI));
}
if (qIsInf(pitch)) {
_pitchFact.setRawValue(0);
} else {
_pitchFact.setRawValue(pitch * (180.0 / M_PI));
}
if (qIsInf(yaw)) {
_headingFact.setRawValue(0);
} else {
yaw = yaw * (180.0 / M_PI);
if (yaw < 0.0) yaw += 360.0;
// truncate to integer so widget never displays 360
_headingFact.setRawValue(trunc(yaw));
}
}
void Vehicle::_updateAttitude(UASInterface* uas, int, double roll, double pitch, double yaw, quint64 timestamp)
{
_updateAttitude(uas, roll, pitch, yaw, timestamp);
}
int Vehicle::motorCount(void)
{
switch (_vehicleType) {
......
src/Vehicle/Vehicle.h
View file @ 161096d0
......@@ -1041,10 +1041,6 @@ private slots:
void _handleTextMessage (int newCount);
void _handletextMessageReceived (UASMessage* message);
/** @brief Attitude from main autopilot / system state */
void _updateAttitude (UASInterface* uas, double roll, double pitch, double yaw, quint64 timestamp);
/** @brief Attitude from one specific component / redundant autopilot */
void _updateAttitude (UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 timestamp);
/** @brief A new camera image has arrived */
void _imageReady (UASInterface* uas);
void _prearmErrorTimeout(void);
......@@ -1089,7 +1085,7 @@ private:
void _handleScaledPressure2(mavlink_message_t& message);
void _handleScaledPressure3(mavlink_message_t& message);
void _handleHighLatency2(mavlink_message_t& message);
void _handleAttitude(mavlink_message_t& message);
void _handleAttitudeQuaternion(mavlink_message_t& message);
void _handleAttitudeTarget(mavlink_message_t& message);
void _handleDistanceSensor(mavlink_message_t& message);
// ArduPilot dialect messages
......
src/uas/UAS.cc
View file @ 161096d0
......@@ -275,86 +275,6 @@ void UAS::receiveMessage(mavlink_message_t message)
emit valueChanged(uasId, name.arg("drop_rate_comm"), "%", state.drop_rate_comm/100.0f, time);
}
break;
case MAVLINK_MSG_ID_ATTITUDE:
{
mavlink_attitude_t attitude;
mavlink_msg_attitude_decode(&message, &attitude);
quint64 time = getUnixReferenceTime(attitude.time_boot_ms);
emit attitudeChanged(this, message.compid, QGC::limitAngleToPMPIf(attitude.roll), QGC::limitAngleToPMPIf(attitude.pitch), QGC::limitAngleToPMPIf(attitude.yaw), time);
if (!wrongComponent)
{
lastAttitude = time;
setRoll(QGC::limitAngleToPMPIf(attitude.roll));
setPitch(QGC::limitAngleToPMPIf(attitude.pitch));
setYaw(QGC::limitAngleToPMPIf(attitude.yaw));
attitudeKnown = true;
emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time);
}
}
break;
case MAVLINK_MSG_ID_ATTITUDE_QUATERNION:
{
mavlink_attitude_quaternion_t attitude;
mavlink_msg_attitude_quaternion_decode(&message, &attitude);
quint64 time = getUnixReferenceTime(attitude.time_boot_ms);
double a = attitude.q1;
double b = attitude.q2;
double c = attitude.q3;
double d = attitude.q4;
double aSq = a * a;
double bSq = b * b;
double cSq = c * c;
double dSq = d * d;
float dcm[3][3];
dcm[0][0] = aSq + bSq - cSq - dSq;
dcm[0][1] = 2.0 * (b * c - a * d);
dcm[0][2] = 2.0 * (a * c + b * d);
dcm[1][0] = 2.0 * (b * c + a * d);
dcm[1][1] = aSq - bSq + cSq - dSq;
dcm[1][2] = 2.0 * (c * d - a * b);
dcm[2][0] = 2.0 * (b * d - a * c);
dcm[2][1] = 2.0 * (a * b + c * d);
dcm[2][2] = aSq - bSq - cSq + dSq;
float phi, theta, psi;
theta = asin(-dcm[2][0]);
if (fabs(theta - M_PI_2) < 1.0e-3f) {
phi = 0.0f;
psi = (atan2(dcm[1][2] - dcm[0][1],
dcm[0][2] + dcm[1][1]) + phi);
} else if (fabs(theta + M_PI_2) < 1.0e-3f) {
phi = 0.0f;
psi = atan2f(dcm[1][2] - dcm[0][1],
dcm[0][2] + dcm[1][1] - phi);
} else {
phi = atan2f(dcm[2][1], dcm[2][2]);
psi = atan2f(dcm[1][0], dcm[0][0]);
}
emit attitudeChanged(this, message.compid, QGC::limitAngleToPMPIf(phi),
QGC::limitAngleToPMPIf(theta),
QGC::limitAngleToPMPIf(psi), time);
if (!wrongComponent)
{
lastAttitude = time;
setRoll(QGC::limitAngleToPMPIf(phi));
setPitch(QGC::limitAngleToPMPIf(theta));
setYaw(QGC::limitAngleToPMPIf(psi));
attitudeKnown = true;
emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time);
}
}
break;
case MAVLINK_MSG_ID_HIL_CONTROLS:
{
mavlink_hil_controls_t hil;
......@@ -362,27 +282,6 @@ void UAS::receiveMessage(mavlink_message_t message)
emit hilControlsChanged(hil.time_usec, hil.roll_ailerons, hil.pitch_elevator, hil.yaw_rudder, hil.throttle, hil.mode, hil.nav_mode);
}
break;
case MAVLINK_MSG_ID_VFR_HUD:
{
mavlink_vfr_hud_t hud;
mavlink_msg_vfr_hud_decode(&message, &hud);
quint64 time = getUnixTime();
if (!attitudeKnown)
{
setYaw(QGC::limitAngleToPMPId((((double)hud.heading)/180.0)*M_PI));
emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time);
}
}
break;
case MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE:
{
mavlink_global_vision_position_estimate_t pos;
mavlink_msg_global_vision_position_estimate_decode(&message, &pos);
quint64 time = getUnixTime(pos.usec);
emit attitudeChanged(this, message.compid, pos.roll, pos.pitch, pos.yaw, time);
}
break;
case MAVLINK_MSG_ID_PARAM_VALUE:
{
......
src/uas/UASInterface.h
View file @ 161096d0
......@@ -213,8 +213,6 @@ signals:
*/
void batteryChanged(UASInterface* uas, double voltage, double current, double percent, int seconds);
void statusChanged(UASInterface* uas, QString status);
void attitudeChanged(UASInterface*, double roll, double pitch, double yaw, quint64 usec);
void attitudeChanged(UASInterface*, int component, double roll, double pitch, double yaw, quint64 usec);
void imageStarted(int imgid, int width, int height, int depth, int channels);
void imageDataReceived(int imgid, const unsigned char* imageData, int length, int startIndex);
......
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