Skip to content

Q
qgroundcontrol
Commit 1bf286ac authored by Don Gagne's avatar Don Gagne
Browse files
Options

New rc signalling in Vehicle

parent ce98f3f1
Hide whitespace changes
Inline Side-by-side
Showing with 139 additions and 116 deletions
src/AutoPilotPlugins/PX4/FlightModesComponentController.cc
View file @ 1bf286ac
......@@ -55,12 +55,7 @@ FlightModesComponentController::FlightModesComponentController(void) :
_init();
_validateConfiguration();
connect(_uas, &UASInterface::remoteControlChannelRawChanged, this, &FlightModesComponentController::_remoteControlChannelRawChanged);
}
FlightModesComponentController::~FlightModesComponentController()
{
disconnect(_uas, &UASInterface::remoteControlChannelRawChanged, this, &FlightModesComponentController::_remoteControlChannelRawChanged);
connect(_vehicle, &Vehicle::rcChannelsChanged, this, &FlightModesComponentController::_rcChannelsChanged);
}
void FlightModesComponentController::_init(void)
......@@ -209,27 +204,29 @@ void FlightModesComponentController::_validateConfiguration(void)
}
}
/// @brief This routine is called whenever a raw value for an RC channel changes.
/// @param chan RC channel on which signal is coming from (0-based)
/// @param fval Current value for channel
void FlightModesComponentController::_remoteControlChannelRawChanged(int chan, float fval)
/// Connected to Vehicle::rcChannelsChanged signal
void FlightModesComponentController::_rcChannelsChanged(int channelCount, int pwmValues[Vehicle::cMaxRcChannels])
{
Q_ASSERT(chan >= 0 && chan <= _chanMax);
if (fval < _rgRCMin[chan]) {
fval= _rgRCMin[chan];
}
if (fval > _rgRCMax[chan]) {
fval= _rgRCMax[chan];
}
float percentRange = (fval - _rgRCMin[chan]) / (float)(_rgRCMax[chan] - _rgRCMin[chan]);
if (_rgRCReversed[chan]) {
percentRange = 1.0 - percentRange;
for (int channel=0; channel<channelCount; channel++) {
int channelValue = pwmValues[channel];
if (channelValue != -1) {
if (channelValue < _rgRCMin[channel]) {
channelValue= _rgRCMin[channel];
}
if (channelValue > _rgRCMax[channel]) {
channelValue= _rgRCMax[channel];
}
float percentRange = (channelValue - _rgRCMin[channel]) / (float)(_rgRCMax[channel] - _rgRCMin[channel]);
if (_rgRCReversed[channel]) {
percentRange = 1.0 - percentRange;
}
_rcValues[channel] = percentRange;
}
}
_rcValues[chan] = percentRange;
_recalcModeSelections();
emit switchLiveRangeChanged();
......
src/AutoPilotPlugins/PX4/FlightModesComponentController.h
View file @ 1bf286ac
......@@ -43,7 +43,6 @@ class FlightModesComponentController : public FactPanelController
public:
FlightModesComponentController(void);
~FlightModesComponentController();
Q_PROPERTY(bool validConfiguration MEMBER _validConfiguration CONSTANT)
Q_PROPERTY(QString configurationErrors MEMBER _configurationErrors CONSTANT)
......@@ -183,7 +182,7 @@ signals:
void modeRowsChanged(void);
private slots:
void _remoteControlChannelRawChanged(int chan, float fval);
void _rcChannelsChanged(int channelCount, int pwmValues[Vehicle::cMaxRcChannels]);
private:
double _switchLiveRange(const QString& param);
......
src/Vehicle/Vehicle.cc
View file @ 1bf286ac
......@@ -119,10 +119,10 @@ Vehicle::Vehicle(LinkInterface* link,
setLatitude(_uas->getLatitude());
setLongitude(_uas->getLongitude());
connect(_uas, &UAS::latitudeChanged, this, &Vehicle::setLatitude);
connect(_uas, &UAS::longitudeChanged, this, &Vehicle::setLongitude);
connect(_uas, &UAS::imageReady, this, &Vehicle::_imageReady);
connect(_uas, &UAS::remoteControlRSSIChanged, this, &Vehicle::_remoteControlRSSIChanged);
connect(_uas, &UAS::latitudeChanged, this, &Vehicle::setLatitude);
connect(_uas, &UAS::longitudeChanged, this, &Vehicle::setLongitude);
connect(_uas, &UAS::imageReady, this, &Vehicle::_imageReady);
connect(this, &Vehicle::remoteControlRSSIChanged, this, &Vehicle::_remoteControlRSSIChanged);
_firmwarePlugin = _firmwarePluginManager->firmwarePluginForAutopilot(_firmwareType, _vehicleType);
_autopilotPlugin = _autopilotPluginManager->newAutopilotPluginForVehicle(this);
......@@ -211,12 +211,18 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
_firmwarePlugin->adjustMavlinkMessage(&message);
switch (message.msgid) {
case MAVLINK_MSG_ID_HOME_POSITION:
_handleHomePosition(message);
break;
case MAVLINK_MSG_ID_HEARTBEAT:
_handleHeartbeat(message);
break;
case MAVLINK_MSG_ID_HOME_POSITION:
_handleHomePosition(message);
break;
case MAVLINK_MSG_ID_HEARTBEAT:
_handleHeartbeat(message);
break;
case MAVLINK_MSG_ID_RC_CHANNELS:
_handleRCChannels(message);
break;
case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
_handleRCChannelsRaw(message);
break;
}
emit mavlinkMessageReceived(message);
......@@ -281,6 +287,89 @@ void Vehicle::_handleHeartbeat(mavlink_message_t& message)
}
}
void Vehicle::_handleRCChannels(mavlink_message_t& message)
{
mavlink_rc_channels_t channels;
mavlink_msg_rc_channels_decode(&message, &channels);
uint16_t* _rgChannelvalues[cMaxRcChannels] = {
&channels.chan1_raw,
&channels.chan2_raw,
&channels.chan3_raw,
&channels.chan4_raw,
&channels.chan5_raw,
&channels.chan6_raw,
&channels.chan7_raw,
&channels.chan8_raw,
&channels.chan9_raw,
&channels.chan10_raw,
&channels.chan11_raw,
&channels.chan12_raw,
&channels.chan13_raw,
&channels.chan14_raw,
&channels.chan15_raw,
&channels.chan16_raw,
&channels.chan17_raw,
&channels.chan18_raw,
};
int pwmValues[cMaxRcChannels];
for (int i=0; i<cMaxRcChannels; i++) {
uint16_t channelValue = *_rgChannelvalues[i];
if (i < channels.chancount) {
pwmValues[i] = channelValue == UINT16_MAX ? -1 : channelValue;
} else {
pwmValues[i] = -1;
}
}
emit remoteControlRSSIChanged(channels.rssi);
emit rcChannelsChanged(channels.chancount, pwmValues);
}
void Vehicle::_handleRCChannelsRaw(mavlink_message_t& message)
{
// We handle both RC_CHANNLES and RC_CHANNELS_RAW since different firmware will only
// send one or the other.
mavlink_rc_channels_raw_t channels;
mavlink_msg_rc_channels_raw_decode(&message, &channels);
uint16_t* _rgChannelvalues[cMaxRcChannels] = {
&channels.chan1_raw,
&channels.chan2_raw,
&channels.chan3_raw,
&channels.chan4_raw,
&channels.chan5_raw,
&channels.chan6_raw,
&channels.chan7_raw,
&channels.chan8_raw,
};
int pwmValues[cMaxRcChannels];
int channelCount = 0;
for (int i=0; i<8; i++) {
uint16_t channelValue = *_rgChannelvalues[i];
if (channelValue == UINT16_MAX) {
pwmValues[i] = -1;
} else {
channelCount = i;
pwmValues[i] = channelValue;
}
}
for (int i=9; i<18; i++) {
pwmValues[i] = -1;
}
emit remoteControlRSSIChanged(channels.rssi);
emit rcChannelsChanged(channelCount, pwmValues);
}
bool Vehicle::_containsLink(LinkInterface* link)
{
return _links.contains(link);
......
src/Vehicle/Vehicle.h
View file @ 1bf286ac
......@@ -112,6 +112,9 @@ public:
Q_PROPERTY(bool active READ active WRITE setActive NOTIFY activeChanged)
Q_PROPERTY(int flowImageIndex READ flowImageIndex NOTIFY flowImageIndexChanged)
Q_PROPERTY(int rcRSSI READ rcRSSI NOTIFY rcRSSIChanged)
Q_PROPERTY(bool px4Firmware READ px4Firmware CONSTANT)
Q_PROPERTY(bool apmFirmware READ apmFirmware CONSTANT)
Q_PROPERTY(bool genericFirmware READ genericFirmware CONSTANT)
/// Returns the number of buttons which are reserved for firmware use in the MANUAL_CONTROL mavlink
/// message. For example PX4 Flight Stack reserves the first 8 buttons to simulate rc switches.
......@@ -253,9 +256,14 @@ public:
int satelliteLock () { return _satelliteLock; }
unsigned int heartbeatTimeout () { return _currentHeartbeatTimeout; }
int rcRSSI () { return _rcRSSI; }
bool px4Firmware () { return _firmwareType == MAV_AUTOPILOT_PX4; }
bool apmFirmware () { return _firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA; }
bool genericFirmware () { return !px4Firmware() && !apmFirmware(); }
ParameterLoader* getParameterLoader(void);
static const int cMaxRcChannels = 18;
public slots:
void setLatitude(double latitude);
void setLongitude(double longitude);
......@@ -305,6 +313,14 @@ signals:
void flowImageIndexChanged ();
void rcRSSIChanged (int rcRSSI);
/// New RC channel values
/// @param channelCount Number of available channels, cMaxRcChannels max
/// @param pwmValues -1 signals channel not available
void rcChannelsChanged(int channelCount, int pwmValues[cMaxRcChannels]);
/// Remote control RSSI changed (0% - 100%)
void remoteControlRSSIChanged(uint8_t rssi);
private slots:
void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message);
void _linkInactiveOrDeleted(LinkInterface* link);
......@@ -342,6 +358,8 @@ private:
void _startJoystick(bool start);
void _handleHomePosition(mavlink_message_t& message);
void _handleHeartbeat(mavlink_message_t& message);
void _handleRCChannels(mavlink_message_t& message);
void _handleRCChannelsRaw(mavlink_message_t& message);
void _missionManagerError(int errorCode, const QString& errorMsg);
void _mapTrajectoryStart(void);
void _mapTrajectoryStop(void);
......
src/uas/UAS.cc
View file @ 1bf286ac
......@@ -755,82 +755,9 @@ void UAS::receiveMessage(mavlink_message_t message)
mavlink_gps_global_origin_t pos;
mavlink_msg_gps_global_origin_decode(&message, &pos);
emit homePositionChanged(uasId, pos.latitude / 10000000.0, pos.longitude / 10000000.0, pos.altitude / 1000.0);
}
break;
case MAVLINK_MSG_ID_RC_CHANNELS:
{
mavlink_rc_channels_t channels;
mavlink_msg_rc_channels_decode(&message, &channels);
emit remoteControlRSSIChanged(channels.rssi);
if (channels.chan1_raw != UINT16_MAX && channels.chancount > 0)
emit remoteControlChannelRawChanged(0, channels.chan1_raw);
if (channels.chan2_raw != UINT16_MAX && channels.chancount > 1)
emit remoteControlChannelRawChanged(1, channels.chan2_raw);
if (channels.chan3_raw != UINT16_MAX && channels.chancount > 2)
emit remoteControlChannelRawChanged(2, channels.chan3_raw);
if (channels.chan4_raw != UINT16_MAX && channels.chancount > 3)
emit remoteControlChannelRawChanged(3, channels.chan4_raw);
if (channels.chan5_raw != UINT16_MAX && channels.chancount > 4)
emit remoteControlChannelRawChanged(4, channels.chan5_raw);
if (channels.chan6_raw != UINT16_MAX && channels.chancount > 5)
emit remoteControlChannelRawChanged(5, channels.chan6_raw);
if (channels.chan7_raw != UINT16_MAX && channels.chancount > 6)
emit remoteControlChannelRawChanged(6, channels.chan7_raw);
if (channels.chan8_raw != UINT16_MAX && channels.chancount > 7)
emit remoteControlChannelRawChanged(7, channels.chan8_raw);
if (channels.chan9_raw != UINT16_MAX && channels.chancount > 8)
emit remoteControlChannelRawChanged(8, channels.chan9_raw);
if (channels.chan10_raw != UINT16_MAX && channels.chancount > 9)
emit remoteControlChannelRawChanged(9, channels.chan10_raw);
if (channels.chan11_raw != UINT16_MAX && channels.chancount > 10)
emit remoteControlChannelRawChanged(10, channels.chan11_raw);
if (channels.chan12_raw != UINT16_MAX && channels.chancount > 11)
emit remoteControlChannelRawChanged(11, channels.chan12_raw);
if (channels.chan13_raw != UINT16_MAX && channels.chancount > 12)
emit remoteControlChannelRawChanged(12, channels.chan13_raw);
if (channels.chan14_raw != UINT16_MAX && channels.chancount > 13)
emit remoteControlChannelRawChanged(13, channels.chan14_raw);
if (channels.chan15_raw != UINT16_MAX && channels.chancount > 14)
emit remoteControlChannelRawChanged(14, channels.chan15_raw);
if (channels.chan16_raw != UINT16_MAX && channels.chancount > 15)
emit remoteControlChannelRawChanged(15, channels.chan16_raw);
if (channels.chan17_raw != UINT16_MAX && channels.chancount > 16)
emit remoteControlChannelRawChanged(16, channels.chan17_raw);
if (channels.chan18_raw != UINT16_MAX && channels.chancount > 17)
emit remoteControlChannelRawChanged(17, channels.chan18_raw);
}
break;
// TODO: (gg 20150420) PX4 Firmware does not seem to send this message. Don't know what to do about it.
case MAVLINK_MSG_ID_RC_CHANNELS_SCALED:
{
mavlink_rc_channels_scaled_t channels;
mavlink_msg_rc_channels_scaled_decode(&message, &channels);
const unsigned int portWidth = 8; // XXX magic number
emit remoteControlRSSIChanged(channels.rssi);
if (static_cast<uint16_t>(channels.chan1_scaled) != UINT16_MAX)
emit remoteControlChannelScaledChanged(channels.port * portWidth + 0, channels.chan1_scaled/10000.0f);
if (static_cast<uint16_t>(channels.chan2_scaled) != UINT16_MAX)
emit remoteControlChannelScaledChanged(channels.port * portWidth + 1, channels.chan2_scaled/10000.0f);
if (static_cast<uint16_t>(channels.chan3_scaled) != UINT16_MAX)
emit remoteControlChannelScaledChanged(channels.port * portWidth + 2, channels.chan3_scaled/10000.0f);
if (static_cast<uint16_t>(channels.chan4_scaled) != UINT16_MAX)
emit remoteControlChannelScaledChanged(channels.port * portWidth + 3, channels.chan4_scaled/10000.0f);
if (static_cast<uint16_t>(channels.chan5_scaled) != UINT16_MAX)
emit remoteControlChannelScaledChanged(channels.port * portWidth + 4, channels.chan5_scaled/10000.0f);
if (static_cast<uint16_t>(channels.chan6_scaled) != UINT16_MAX)
emit remoteControlChannelScaledChanged(channels.port * portWidth + 5, channels.chan6_scaled/10000.0f);
if (static_cast<uint16_t>(channels.chan7_scaled) != UINT16_MAX)
emit remoteControlChannelScaledChanged(channels.port * portWidth + 6, channels.chan7_scaled/10000.0f);
if (static_cast<uint16_t>(channels.chan8_scaled) != UINT16_MAX)
emit remoteControlChannelScaledChanged(channels.port * portWidth + 7, channels.chan8_scaled/10000.0f);
}
break;
case MAVLINK_MSG_ID_PARAM_VALUE:
{
mavlink_param_value_t rawValue;
......
src/uas/UASInterface.h
View file @ 1bf286ac
......@@ -302,13 +302,6 @@ signals:
/** @brief Differential pressure / airspeed status changed */
void airspeedStatusChanged(bool supported, bool enabled, bool ok);
/** @brief Value of a remote control channel (raw) */
void remoteControlChannelRawChanged(int channelId, float raw);
/** @brief Value of a remote control channel (scaled)*/
void remoteControlChannelScaledChanged(int channelId, float normalized);
/** @brief Remote control RSSI changed (0% - 100%)*/
void remoteControlRSSIChanged(uint8_t rssi);
/**
* @brief Localization quality changed
* @param fix 0: lost, 1: 2D local position hold, 2: 2D localization, 3: 3D localization
......
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