@ -139,9 +139,15 @@ QString APMCustomMode::modeString() const
@@ -139,9 +139,15 @@ QString APMCustomMode::modeString() const
return mode ;
}
APMFirmwarePluginInstanceData : : APMFirmwarePluginInstanceData ( QObject * parent )
: QObject ( parent )
, textSeverityAdjustmentNeeded ( false )
{
}
APMFirmwarePlugin : : APMFirmwarePlugin ( void )
: _coaxialMotors ( false )
, _textSeverityAdjustmentNeeded ( false )
{
qmlRegisterType < APMFlightModesComponentController > ( " QGroundControl.Controllers " , 1 , 0 , " APMFlightModesComponentController " ) ;
qmlRegisterType < APMAirframeComponentController > ( " QGroundControl.Controllers " , 1 , 0 , " APMAirframeComponentController " ) ;
@ -321,6 +327,7 @@ void APMFirmwarePlugin::_handleOutgoingParamSet(Vehicle* vehicle, LinkInterface*
@@ -321,6 +327,7 @@ void APMFirmwarePlugin::_handleOutgoingParamSet(Vehicle* vehicle, LinkInterface*
bool APMFirmwarePlugin : : _handleIncomingStatusText ( Vehicle * vehicle , mavlink_message_t * message )
{
QString messageText ;
APMFirmwarePluginInstanceData * instanceData = qobject_cast < APMFirmwarePluginInstanceData * > ( vehicle - > firmwarePluginInstanceData ( ) ) ;
mavlink_statustext_t statusText ;
mavlink_msg_statustext_decode ( message , & statusText ) ;
@ -334,7 +341,7 @@ bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* vehicle, mavlink_mess
@@ -334,7 +341,7 @@ bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* vehicle, mavlink_mess
if ( messageText . contains ( APM_COPTER_REXP ) | | messageText . contains ( APM_PLANE_REXP ) | | messageText . contains ( APM_ROVER_REXP ) | | messageText . contains ( APM_SUB_REXP ) ) {
// found version string
APMFirmwareVersion firmwareVersion ( messageText ) ;
_ textSeverityAdjustmentNeeded = _isTextSeverityAdjustmentNeeded ( firmwareVersion ) ;
instanceData - > textSeverityAdjustmentNeeded = _isTextSeverityAdjustmentNeeded ( firmwareVersion ) ;
vehicle - > setFirmwareVersion ( firmwareVersion . majorNumber ( ) , firmwareVersion . minorNumber ( ) , firmwareVersion . patchNumber ( ) ) ;
@ -378,7 +385,7 @@ bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* vehicle, mavlink_mess
@@ -378,7 +385,7 @@ bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* vehicle, mavlink_mess
}
// adjust mesasge if needed
if ( _ textSeverityAdjustmentNeeded) {
if ( instanceData - > textSeverityAdjustmentNeeded ) {
_adjustSeverity ( message ) ;
}
@ -416,10 +423,10 @@ bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* vehicle, mavlink_mess
@@ -416,10 +423,10 @@ bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* vehicle, mavlink_mess
if ( messageText . startsWith ( " PreArm " ) ) {
// ArduPilot PreArm messages can come across very frequently especially on Solo, which seems to send them once a second.
// Filter them out if they come too quickly.
if ( _ noisyPrearmMap. contains ( messageText ) & & _ noisyPrearmMap[ messageText ] . msecsTo ( QTime : : currentTime ( ) ) < ( 10 * 1000 ) ) {
if ( instanceData - > noisyPrearmMap . contains ( messageText ) & & instanceData - > noisyPrearmMap [ messageText ] . msecsTo ( QTime : : currentTime ( ) ) < ( 10 * 1000 ) ) {
return false ;
}
_ noisyPrearmMap[ messageText ] = QTime : : currentTime ( ) ;
instanceData - > noisyPrearmMap [ messageText ] = QTime : : currentTime ( ) ;
vehicle - > setPrearmError ( messageText ) ;
}
@ -578,6 +585,8 @@ void APMFirmwarePlugin::_adjustCalibrationMessageSeverity(mavlink_message_t* mes
@@ -578,6 +585,8 @@ void APMFirmwarePlugin::_adjustCalibrationMessageSeverity(mavlink_message_t* mes
void APMFirmwarePlugin : : initializeVehicle ( Vehicle * vehicle )
{
vehicle - > setFirmwarePluginInstanceData ( new APMFirmwarePluginInstanceData ) ;
if ( vehicle - > isOfflineEditingVehicle ( ) ) {
switch ( vehicle - > vehicleType ( ) ) {
case MAV_TYPE_QUADROTOR :