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

New rc signalling in Vehicle

QGC4.4
Don Gagne 10 years ago
parent
ce98f3f1c0
commit
1bf286acd0
6 changed files with 139 additions and 116 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 33
      src/AutoPilotPlugins/PX4/FlightModesComponentController.cc
  2. 3
      src/AutoPilotPlugins/PX4/FlightModesComponentController.h
  3. 91
      src/Vehicle/Vehicle.cc
  4. 18
      src/Vehicle/Vehicle.h
  5. 75
      src/uas/UAS.cc
  6. 7
      src/uas/UASInterface.h

33
src/AutoPilotPlugins/PX4/FlightModesComponentController.cc
Unescape View File

@ -55,12 +55,7 @@ FlightModesComponentController::FlightModesComponentController(void) :
_init(); _init();
_validateConfiguration(); _validateConfiguration();
connect(_uas, &UASInterface::remoteControlChannelRawChanged, this, &FlightModesComponentController::_remoteControlChannelRawChanged); connect(_vehicle, &Vehicle::rcChannelsChanged, this, &FlightModesComponentController::_rcChannelsChanged);
}
FlightModesComponentController::~FlightModesComponentController()
{
disconnect(_uas, &UASInterface::remoteControlChannelRawChanged, this, &FlightModesComponentController::_remoteControlChannelRawChanged);
} }
void FlightModesComponentController::_init(void) void FlightModesComponentController::_init(void)
@ -209,26 +204,28 @@ void FlightModesComponentController::_validateConfiguration(void)
} }
} }
/// @brief This routine is called whenever a raw value for an RC channel changes. /// Connected to Vehicle::rcChannelsChanged signal
/// @param chan RC channel on which signal is coming from (0-based) void FlightModesComponentController::_rcChannelsChanged(int channelCount, int pwmValues[Vehicle::cMaxRcChannels])
/// @param fval Current value for channel
void FlightModesComponentController::_remoteControlChannelRawChanged(int chan, float fval)
{ {
Q_ASSERT(chan >= 0 && chan <= _chanMax); for (int channel=0; channel<channelCount; channel++) {
int channelValue = pwmValues[channel];
if (fval < _rgRCMin[chan]) { if (channelValue != -1) {
fval= _rgRCMin[chan]; if (channelValue < _rgRCMin[channel]) {
channelValue= _rgRCMin[channel];
} }
if (fval > _rgRCMax[chan]) { if (channelValue > _rgRCMax[channel]) {
fval= _rgRCMax[chan]; channelValue= _rgRCMax[channel];
} }
float percentRange = (fval - _rgRCMin[chan]) / (float)(_rgRCMax[chan] - _rgRCMin[chan]); float percentRange = (channelValue - _rgRCMin[channel]) / (float)(_rgRCMax[channel] - _rgRCMin[channel]);
if (_rgRCReversed[chan]) { if (_rgRCReversed[channel]) {
percentRange = 1.0 - percentRange; percentRange = 1.0 - percentRange;
} }
_rcValues[chan] = percentRange; _rcValues[channel] = percentRange;
}
}
_recalcModeSelections(); _recalcModeSelections();

3
src/AutoPilotPlugins/PX4/FlightModesComponentController.h
Unescape View File

@ -43,7 +43,6 @@ class FlightModesComponentController : public FactPanelController
public: public:
FlightModesComponentController(void); FlightModesComponentController(void);
~FlightModesComponentController();
Q_PROPERTY(bool validConfiguration MEMBER _validConfiguration CONSTANT) Q_PROPERTY(bool validConfiguration MEMBER _validConfiguration CONSTANT)
Q_PROPERTY(QString configurationErrors MEMBER _configurationErrors CONSTANT) Q_PROPERTY(QString configurationErrors MEMBER _configurationErrors CONSTANT)
@ -183,7 +182,7 @@ signals:
void modeRowsChanged(void); void modeRowsChanged(void);
private slots: private slots:
void _remoteControlChannelRawChanged(int chan, float fval); void _rcChannelsChanged(int channelCount, int pwmValues[Vehicle::cMaxRcChannels]);
private: private:
double _switchLiveRange(const QString& param); double _switchLiveRange(const QString& param);

91
src/Vehicle/Vehicle.cc
Unescape View File

@ -122,7 +122,7 @@ Vehicle::Vehicle(LinkInterface* link,
connect(_uas, &UAS::latitudeChanged, this, &Vehicle::setLatitude); connect(_uas, &UAS::latitudeChanged, this, &Vehicle::setLatitude);
connect(_uas, &UAS::longitudeChanged, this, &Vehicle::setLongitude); connect(_uas, &UAS::longitudeChanged, this, &Vehicle::setLongitude);
connect(_uas, &UAS::imageReady, this, &Vehicle::_imageReady); connect(_uas, &UAS::imageReady, this, &Vehicle::_imageReady);
connect(_uas, &UAS::remoteControlRSSIChanged, this, &Vehicle::_remoteControlRSSIChanged); connect(this, &Vehicle::remoteControlRSSIChanged, this, &Vehicle::_remoteControlRSSIChanged);
_firmwarePlugin = _firmwarePluginManager->firmwarePluginForAutopilot(_firmwareType, _vehicleType); _firmwarePlugin = _firmwarePluginManager->firmwarePluginForAutopilot(_firmwareType, _vehicleType);
_autopilotPlugin = _autopilotPluginManager->newAutopilotPluginForVehicle(this); _autopilotPlugin = _autopilotPluginManager->newAutopilotPluginForVehicle(this);
@ -217,6 +217,12 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
case MAVLINK_MSG_ID_HEARTBEAT: case MAVLINK_MSG_ID_HEARTBEAT:
_handleHeartbeat(message); _handleHeartbeat(message);
break; break;
case MAVLINK_MSG_ID_RC_CHANNELS:
_handleRCChannels(message);
break;
case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
_handleRCChannelsRaw(message);
break;
} }
emit mavlinkMessageReceived(message); 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) bool Vehicle::_containsLink(LinkInterface* link)
{ {
return _links.contains(link); return _links.contains(link);

18
src/Vehicle/Vehicle.h
Unescape View File

@ -112,6 +112,9 @@ public:
Q_PROPERTY(bool active READ active WRITE setActive NOTIFY activeChanged) Q_PROPERTY(bool active READ active WRITE setActive NOTIFY activeChanged)
Q_PROPERTY(int flowImageIndex READ flowImageIndex NOTIFY flowImageIndexChanged) Q_PROPERTY(int flowImageIndex READ flowImageIndex NOTIFY flowImageIndexChanged)
Q_PROPERTY(int rcRSSI READ rcRSSI NOTIFY rcRSSIChanged) 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 /// 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. /// message. For example PX4 Flight Stack reserves the first 8 buttons to simulate rc switches.
@ -253,9 +256,14 @@ public:
int satelliteLock () { return _satelliteLock; } int satelliteLock () { return _satelliteLock; }
unsigned int heartbeatTimeout () { return _currentHeartbeatTimeout; } unsigned int heartbeatTimeout () { return _currentHeartbeatTimeout; }
int rcRSSI () { return _rcRSSI; } 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); ParameterLoader* getParameterLoader(void);
static const int cMaxRcChannels = 18;
public slots: public slots:
void setLatitude(double latitude); void setLatitude(double latitude);
void setLongitude(double longitude); void setLongitude(double longitude);
@ -305,6 +313,14 @@ signals:
void flowImageIndexChanged (); void flowImageIndexChanged ();
void rcRSSIChanged (int rcRSSI); 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: private slots:
void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message); void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message);
void _linkInactiveOrDeleted(LinkInterface* link); void _linkInactiveOrDeleted(LinkInterface* link);
@ -342,6 +358,8 @@ private:
void _startJoystick(bool start); void _startJoystick(bool start);
void _handleHomePosition(mavlink_message_t& message); void _handleHomePosition(mavlink_message_t& message);
void _handleHeartbeat(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 _missionManagerError(int errorCode, const QString& errorMsg);
void _mapTrajectoryStart(void); void _mapTrajectoryStart(void);
void _mapTrajectoryStop(void); void _mapTrajectoryStop(void);

75
src/uas/UAS.cc
Unescape View File

@ -757,80 +757,7 @@ void UAS::receiveMessage(mavlink_message_t message)
emit homePositionChanged(uasId, pos.latitude / 10000000.0, pos.longitude / 10000000.0, pos.altitude / 1000.0); emit homePositionChanged(uasId, pos.latitude / 10000000.0, pos.longitude / 10000000.0, pos.altitude / 1000.0);
} }
break; 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: case MAVLINK_MSG_ID_PARAM_VALUE:
{ {
mavlink_param_value_t rawValue; mavlink_param_value_t rawValue;

7
src/uas/UASInterface.h
Unescape View File

@ -302,13 +302,6 @@ signals:
/** @brief Differential pressure / airspeed status changed */ /** @brief Differential pressure / airspeed status changed */
void airspeedStatusChanged(bool supported, bool enabled, bool ok); 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 * @brief Localization quality changed
* @param fix 0: lost, 1: 2D local position hold, 2: 2D localization, 3: 3D localization * @param fix 0: lost, 1: 2D local position hold, 2: 2D localization, 3: 3D localization

Write Preview
Loading…
Cancel
Save