Commit 56d400b0 authored by Bart Slinger's avatar Bart Slinger

using Vehicle instead of UAS

parent ae153170
Subproject commit 621bb1f70d17548b3f159fb30514023960be41f1
Subproject commit e93ac62981a338a7c823364e7c4ff1077e3f8fc1
......@@ -469,6 +469,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
case MAVLINK_MSG_ID_WIND_COV:
_handleWindCov(message);
break;
case MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS:
_handleHilActuatorControls(message);
break;
// Following are ArduPilot dialect messages
......@@ -499,6 +502,30 @@ void Vehicle::_handleAutopilotVersion(mavlink_message_t& message)
}
}
void Vehicle::_handleHilActuatorControls(mavlink_message_t &message)
{
mavlink_hil_actuator_controls_t hil;
mavlink_msg_hil_actuator_controls_decode(&message, &hil);
emit hilActuatorControlsChanged(hil.time_usec, hil.flags,
hil.controls[0],
hil.controls[1],
hil.controls[2],
hil.controls[3],
hil.controls[4],
hil.controls[5],
hil.controls[6],
hil.controls[7],
hil.controls[8],
hil.controls[9],
hil.controls[10],
hil.controls[11],
hil.controls[12],
hil.controls[13],
hil.controls[14],
hil.controls[15],
hil.mode);
}
void Vehicle::_handleCommandAck(mavlink_message_t& message)
{
mavlink_command_ack_t ack;
......
......@@ -683,6 +683,7 @@ private:
void _handleExtendedSysState(mavlink_message_t& message);
void _handleCommandAck(mavlink_message_t& message);
void _handleAutopilotVersion(mavlink_message_t& message);
void _handleHilActuatorControls(mavlink_message_t& message);
void _missionManagerError(int errorCode, const QString& errorMsg);
void _geoFenceManagerError(int errorCode, const QString& errorMsg);
void _mapTrajectoryStart(void);
......
......@@ -190,7 +190,7 @@ void QGCXPlaneLink::run()
QObject::connect(socket, &QUdpSocket::readyRead, this, &QGCXPlaneLink::readBytes);
connect(_vehicle->uas(), &UAS::hilControlsChanged, this, &QGCXPlaneLink::updateControls, Qt::QueuedConnection);
connect(_vehicle->uas(), &UAS::hilActuatorControlsChanged, this, &QGCXPlaneLink::updateActuatorControls, Qt::QueuedConnection);
connect(_vehicle, &Vehicle::hilActuatorControlsChanged, this, &QGCXPlaneLink::updateActuatorControls, Qt::QueuedConnection);
connect(this, &QGCXPlaneLink::hilGroundTruthChanged, _vehicle->uas(), &UAS::sendHilGroundTruth, Qt::QueuedConnection);
connect(this, &QGCXPlaneLink::hilStateChanged, _vehicle->uas(), &UAS::sendHilState, Qt::QueuedConnection);
......@@ -504,6 +504,10 @@ void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ct
sendDataRef("sim/flightmodel/controls/wing1r_ail1def", ctl_9 * max_surface_deflection);
sendDataRef("sim/flightmodel/controls/wing2l_ail1def", ctl_10 * max_surface_deflection);
sendDataRef("sim/flightmodel/controls/wing2r_ail1def", ctl_11 * max_surface_deflection);
sendDataRef("sim/flightmodel/controls/wing1l_ail2def", ctl_12 * max_surface_deflection);
sendDataRef("sim/flightmodel/controls/wing1r_ail2def", ctl_13 * max_surface_deflection);
sendDataRef("sim/flightmodel/controls/wing2l_ail2def", ctl_14 * max_surface_deflection);
sendDataRef("sim/flightmodel/controls/wing2r_ail2def", ctl_15 * max_surface_deflection);
break;
}
......
......@@ -429,30 +429,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_HIL_ACTUATOR_CONTROLS:
{
mavlink_hil_actuator_controls_t hil;
mavlink_msg_hil_actuator_controls_decode(&message, &hil);
emit hilActuatorControlsChanged(hil.time_usec, hil.flags,
hil.controls[0],
hil.controls[1],
hil.controls[2],
hil.controls[3],
hil.controls[4],
hil.controls[5],
hil.controls[6],
hil.controls[7],
hil.controls[8],
hil.controls[9],
hil.controls[10],
hil.controls[11],
hil.controls[12],
hil.controls[13],
hil.controls[14],
hil.controls[15],
hil.mode);
}
break;
case MAVLINK_MSG_ID_VFR_HUD:
{
mavlink_vfr_hud_t hud;
......
......@@ -554,8 +554,6 @@ signals:
void imageReady(UASInterface* uas);
/** @brief HIL controls have changed */
void hilControlsChanged(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode);
/** @brief HIL actuator controls (replaces HIL controls) */
void hilActuatorControlsChanged(quint64 time, quint64 flags, float ctl_0, float ctl_1, float ctl_2, float ctl_3, float ctl_4, float ctl_5, float ctl_6, float ctl_7, float ctl_8, float ctl_9, float ctl_10, float ctl_11, float ctl_12, float ctl_13, float ctl_14, float ctl_15, quint8 mode);
void localXChanged(double val,QString name);
void localYChanged(double val,QString name);
......
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