@ -1,24 +1,24 @@
/*=====================================================================
/*=====================================================================
QGroundControl Open Source Ground Control Station QGroundControl Open Source Ground Control Station
( c ) 2009 - 2014 QGROUNDCONTROL PROJECT < http : //www.qgroundcontrol.org>
( c ) 2009 - 2014 QGROUNDCONTROL PROJECT < http : //www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL is free software : you can redistribute it and / or modify QGROUNDCONTROL is free software : you can redistribute it and / or modify
it under the terms of the GNU General Public License as published by it under the terms of the GNU General Public License as published by
the Free Software Foundation , either version 3 of the License , or the Free Software Foundation , either version 3 of the License , or
( at your option ) any later version . ( at your option ) any later version .
QGROUNDCONTROL is distributed in the hope that it will be useful , QGROUNDCONTROL is distributed in the hope that it will be useful ,
but WITHOUT ANY WARRANTY ; without even the implied warranty of but WITHOUT ANY WARRANTY ; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE . See the MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE . See the
GNU General Public License for more details . GNU General Public License for more details .
You should have received a copy of the GNU General Public License You should have received a copy of the GNU General Public License
along with QGROUNDCONTROL . If not , see < http : //www.gnu.org/licenses/>.
along with QGROUNDCONTROL . If not , see < http : //www.gnu.org/licenses/>.
= = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = */ = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = */
/// @file
/// @file
@ -59,10 +59,10 @@ ParameterLoader::ParameterLoader(AutoPilotPlugin* autopilot, Vehicle* vehicle, Q
Q_ASSERT ( _autopilot ) ; Q_ASSERT ( _autopilot ) ;
Q_ASSERT ( _vehicle ) ; Q_ASSERT ( _vehicle ) ;
Q_ASSERT ( _mavlink ) ; Q_ASSERT ( _mavlink ) ;
// We signal this to ouselves in order to start timer on our thread
// We signal this to ouselves in order to start timer on our thread
connect ( this , & ParameterLoader : : restartWaitingParamTimer , this , & ParameterLoader : : _restartWaitingParamTimer ) ; connect ( this , & ParameterLoader : : restartWaitingParamTimer , this , & ParameterLoader : : _restartWaitingParamTimer ) ;
_waitingParamTimeoutTimer . setSingleShot ( true ) ; _waitingParamTimeoutTimer . setSingleShot ( true ) ;
_waitingParamTimeoutTimer . setInterval ( 1000 ) ; _waitingParamTimeoutTimer . setInterval ( 1000 ) ;
connect ( & _waitingParamTimeoutTimer , & QTimer : : timeout , this , & ParameterLoader : : _waitingParamTimeout ) ; connect ( & _waitingParamTimeoutTimer , & QTimer : : timeout , this , & ParameterLoader : : _waitingParamTimeout ) ;
@ -70,7 +70,7 @@ ParameterLoader::ParameterLoader(AutoPilotPlugin* autopilot, Vehicle* vehicle, Q
_cacheTimeoutTimer . setSingleShot ( true ) ; _cacheTimeoutTimer . setSingleShot ( true ) ;
_cacheTimeoutTimer . setInterval ( 2500 ) ; _cacheTimeoutTimer . setInterval ( 2500 ) ;
connect ( & _cacheTimeoutTimer , & QTimer : : timeout , this , & ParameterLoader : : refreshAllParameters ) ; connect ( & _cacheTimeoutTimer , & QTimer : : timeout , this , & ParameterLoader : : refreshAllParameters ) ;
// FIXME: Why not direct connect?
// FIXME: Why not direct connect?
connect ( _vehicle - > uas ( ) , SIGNAL ( parameterUpdate ( int , int , QString , int , int , int , QVariant ) ) , this , SLOT ( _parameterUpdate ( int , int , QString , int , int , int , QVariant ) ) ) ; connect ( _vehicle - > uas ( ) , SIGNAL ( parameterUpdate ( int , int , QString , int , int , int , QVariant ) ) , this , SLOT ( _parameterUpdate ( int , int , QString , int , int , int , QVariant ) ) ) ;
@ -87,12 +87,12 @@ ParameterLoader::~ParameterLoader()
void ParameterLoader : : _parameterUpdate ( int uasId , int componentId , QString parameterName , int parameterCount , int parameterId , int mavType , QVariant value ) void ParameterLoader : : _parameterUpdate ( int uasId , int componentId , QString parameterName , int parameterCount , int parameterId , int mavType , QVariant value )
{ {
bool setMetaData = false ; bool setMetaData = false ;
// Is this for our uas?
// Is this for our uas?
if ( uasId ! = _vehicle - > id ( ) ) { if ( uasId ! = _vehicle - > id ( ) ) {
return ; return ;
} }
qCDebug ( ParameterLoaderLog ) < < " _parameterUpdate (usaId: " < < uasId < < qCDebug ( ParameterLoaderLog ) < < " _parameterUpdate (usaId: " < < uasId < <
" componentId: " < < componentId < < " componentId: " < < componentId < <
" name: " < < parameterName < < " name: " < < parameterName < <
@ -101,7 +101,7 @@ void ParameterLoader::_parameterUpdate(int uasId, int componentId, QString param
" mavType: " < < mavType < < " mavType: " < < mavType < <
" value: " < < value < < " value: " < < value < <
" ) " ; " ) " ;
#if 0 #if 0
// Handy for testing retry logic
// Handy for testing retry logic
static int counter = 0 ; static int counter = 0 ;
@ -118,10 +118,10 @@ void ParameterLoader::_parameterUpdate(int uasId, int componentId, QString param
return ; return ;
} }
_dataMutex . lock ( ) ; _dataMutex . lock ( ) ;
// Restart our waiting for param timer
// Restart our waiting for param timer
_waitingParamTimeoutTimer . start ( ) ; _waitingParamTimeoutTimer . start ( ) ;
// Update our total parameter counts
// Update our total parameter counts
if ( ! _paramCountMap . contains ( componentId ) ) { if ( ! _paramCountMap . contains ( componentId ) ) {
_paramCountMap [ componentId ] = parameterCount ; _paramCountMap [ componentId ] = parameterCount ;
@ -129,7 +129,7 @@ void ParameterLoader::_parameterUpdate(int uasId, int componentId, QString param
} }
_mapParameterId2Name [ componentId ] [ parameterId ] = parameterName ; _mapParameterId2Name [ componentId ] [ parameterId ] = parameterName ;
// If we've never seen this component id before, setup the wait lists.
// If we've never seen this component id before, setup the wait lists.
if ( ! _waitingReadParamIndexMap . contains ( componentId ) ) { if ( ! _waitingReadParamIndexMap . contains ( componentId ) ) {
// Add all indices to the wait list, parameter index is 0-based
// Add all indices to the wait list, parameter index is 0-based
@ -137,14 +137,14 @@ void ParameterLoader::_parameterUpdate(int uasId, int componentId, QString param
// This will add the new component id, as well as the the new waiting index and set the retry count for that index to 0
// This will add the new component id, as well as the the new waiting index and set the retry count for that index to 0
_waitingReadParamIndexMap [ componentId ] [ waitingIndex ] = 0 ; _waitingReadParamIndexMap [ componentId ] [ waitingIndex ] = 0 ;
} }
// The read and write waiting lists for this component are initialized the empty
// The read and write waiting lists for this component are initialized the empty
_waitingReadParamNameMap [ componentId ] = QMap < QString , int > ( ) ; _waitingReadParamNameMap [ componentId ] = QMap < QString , int > ( ) ;
_waitingWriteParamNameMap [ componentId ] = QMap < QString , int > ( ) ; _waitingWriteParamNameMap [ componentId ] = QMap < QString , int > ( ) ;
qCDebug ( ParameterLoaderLog ) < < " Seeing component for first time, id: " < < componentId < < " parameter count: " < < parameterCount ; qCDebug ( ParameterLoaderLog ) < < " Seeing component for first time, id: " < < componentId < < " parameter count: " < < parameterCount ;
} }
// Remove this parameter from the waiting lists
// Remove this parameter from the waiting lists
_waitingReadParamIndexMap [ componentId ] . remove ( parameterId ) ; _waitingReadParamIndexMap [ componentId ] . remove ( parameterId ) ;
_waitingReadParamNameMap [ componentId ] . remove ( parameterName ) ; _waitingReadParamNameMap [ componentId ] . remove ( parameterName ) ;
@ -154,11 +154,11 @@ void ParameterLoader::_parameterUpdate(int uasId, int componentId, QString param
qCDebug ( ParameterLoaderLog ) < < " _waitingWriteParamNameMap " < < _waitingWriteParamNameMap [ componentId ] ; qCDebug ( ParameterLoaderLog ) < < " _waitingWriteParamNameMap " < < _waitingWriteParamNameMap [ componentId ] ;
// Track how many parameters we are still waiting for
// Track how many parameters we are still waiting for
int waitingReadParamIndexCount = 0 ; int waitingReadParamIndexCount = 0 ;
int waitingReadParamNameCount = 0 ; int waitingReadParamNameCount = 0 ;
int waitingWriteParamNameCount = 0 ; int waitingWriteParamNameCount = 0 ;
foreach ( int waitingComponentId , _waitingReadParamIndexMap . keys ( ) ) { foreach ( int waitingComponentId , _waitingReadParamIndexMap . keys ( ) ) {
waitingReadParamIndexCount + = _waitingReadParamIndexMap [ waitingComponentId ] . count ( ) ; waitingReadParamIndexCount + = _waitingReadParamIndexMap [ waitingComponentId ] . count ( ) ;
} }
@ -166,21 +166,21 @@ void ParameterLoader::_parameterUpdate(int uasId, int componentId, QString param
qCDebug ( ParameterLoaderLog ) < < " waitingReadParamIndexCount: " < < waitingReadParamIndexCount ; qCDebug ( ParameterLoaderLog ) < < " waitingReadParamIndexCount: " < < waitingReadParamIndexCount ;
} }
foreach ( int waitingComponentId , _waitingReadParamNameMap . keys ( ) ) { foreach ( int waitingComponentId , _waitingReadParamNameMap . keys ( ) ) {
waitingReadParamNameCount + = _waitingReadParamNameMap [ waitingComponentId ] . count ( ) ; waitingReadParamNameCount + = _waitingReadParamNameMap [ waitingComponentId ] . count ( ) ;
} }
if ( waitingReadParamNameCount ) { if ( waitingReadParamNameCount ) {
qCDebug ( ParameterLoaderLog ) < < " waitingReadParamNameCount: " < < waitingReadParamNameCount ; qCDebug ( ParameterLoaderLog ) < < " waitingReadParamNameCount: " < < waitingReadParamNameCount ;
} }
foreach ( int waitingComponentId , _waitingWriteParamNameMap . keys ( ) ) { foreach ( int waitingComponentId , _waitingWriteParamNameMap . keys ( ) ) {
waitingWriteParamNameCount + = _waitingWriteParamNameMap [ waitingComponentId ] . count ( ) ; waitingWriteParamNameCount + = _waitingWriteParamNameMap [ waitingComponentId ] . count ( ) ;
} }
if ( waitingWriteParamNameCount ) { if ( waitingWriteParamNameCount ) {
qCDebug ( ParameterLoaderLog ) < < " waitingWriteParamNameCount: " < < waitingWriteParamNameCount ; qCDebug ( ParameterLoaderLog ) < < " waitingWriteParamNameCount: " < < waitingWriteParamNameCount ;
} }
int waitingParamCount = waitingReadParamIndexCount + waitingReadParamNameCount + waitingWriteParamNameCount ; int waitingParamCount = waitingReadParamIndexCount + waitingReadParamNameCount + waitingWriteParamNameCount ;
if ( waitingParamCount ) { if ( waitingParamCount ) {
qCDebug ( ParameterLoaderLog ) < < " waitingParamCount: " < < waitingParamCount ; qCDebug ( ParameterLoaderLog ) < < " waitingParamCount: " < < waitingParamCount ;
@ -195,7 +195,7 @@ void ParameterLoader::_parameterUpdate(int uasId, int componentId, QString param
} else { } else {
emit parameterListProgress ( ( float ) ( _totalParamCount - waitingParamCount ) / ( float ) _totalParamCount ) ; emit parameterListProgress ( ( float ) ( _totalParamCount - waitingParamCount ) / ( float ) _totalParamCount ) ;
} }
// Attempt to determine default component id
// Attempt to determine default component id
if ( _defaultComponentId = = FactSystem : : defaultComponentId & & _defaultComponentIdParam . isEmpty ( ) ) { if ( _defaultComponentId = = FactSystem : : defaultComponentId & & _defaultComponentIdParam . isEmpty ( ) ) {
_defaultComponentIdParam = _vehicle - > firmwarePlugin ( ) - > getDefaultComponentIdParam ( ) ; _defaultComponentIdParam = _vehicle - > firmwarePlugin ( ) - > getDefaultComponentIdParam ( ) ;
@ -203,10 +203,10 @@ void ParameterLoader::_parameterUpdate(int uasId, int componentId, QString param
if ( ! _defaultComponentIdParam . isEmpty ( ) & & _defaultComponentIdParam = = parameterName ) { if ( ! _defaultComponentIdParam . isEmpty ( ) & & _defaultComponentIdParam = = parameterName ) {
_defaultComponentId = componentId ; _defaultComponentId = componentId ;
} }
if ( ! _mapParameterName2Variant . contains ( componentId ) | | ! _mapParameterName2Variant [ componentId ] . contains ( parameterName ) ) { if ( ! _mapParameterName2Variant . contains ( componentId ) | | ! _mapParameterName2Variant [ componentId ] . contains ( parameterName ) ) {
qCDebug ( ParameterLoaderLog ) < < " Adding new fact " ; qCDebug ( ParameterLoaderLog ) < < " Adding new fact " ;
FactMetaData : : ValueType_t factType ; FactMetaData : : ValueType_t factType ;
switch ( mavType ) { switch ( mavType ) {
case MAV_PARAM_TYPE_UINT8 : case MAV_PARAM_TYPE_UINT8 :
@ -238,34 +238,34 @@ void ParameterLoader::_parameterUpdate(int uasId, int componentId, QString param
qCritical ( ) < < " Unsupported fact type " < < mavType ; qCritical ( ) < < " Unsupported fact type " < < mavType ;
break ; break ;
} }
Fact * fact = new Fact ( componentId , parameterName , factType , this ) ; Fact * fact = new Fact ( componentId , parameterName , factType , this ) ;
setMetaData = true ; setMetaData = true ;
_mapParameterName2Variant [ componentId ] [ parameterName ] = QVariant : : fromValue ( fact ) ; _mapParameterName2Variant [ componentId ] [ parameterName ] = QVariant : : fromValue ( fact ) ;
// We need to know when the fact changes from QML so that we can send the new value to the parameter manager
// We need to know when the fact changes from QML so that we can send the new value to the parameter manager
connect ( fact , & Fact : : _containerValueChanged , this , & ParameterLoader : : _valueUpdated ) ; connect ( fact , & Fact : : _containerValueChanged , this , & ParameterLoader : : _valueUpdated ) ;
} }
Q_ASSERT ( _mapParameterName2Variant [ componentId ] . contains ( parameterName ) ) ; Q_ASSERT ( _mapParameterName2Variant [ componentId ] . contains ( parameterName ) ) ;
Fact * fact = _mapParameterName2Variant [ componentId ] [ parameterName ] . value < Fact * > ( ) ; Fact * fact = _mapParameterName2Variant [ componentId ] [ parameterName ] . value < Fact * > ( ) ;
Q_ASSERT ( fact ) ; Q_ASSERT ( fact ) ;
fact - > _containerSetValue ( value ) ; fact - > _containerSetValue ( value ) ;
if ( setMetaData ) { if ( setMetaData ) {
_vehicle - > firmwarePlugin ( ) - > addMetaDataToFact ( fact ) ; _vehicle - > firmwarePlugin ( ) - > addMetaDataToFact ( fact ) ;
} }
_dataMutex . unlock ( ) ; _dataMutex . unlock ( ) ;
if ( waitingParamCount = = 0 ) { if ( waitingParamCount = = 0 ) {
// Now that we know vehicle is up to date persist
// Now that we know vehicle is up to date persist
_saveToEEPROM ( ) ; _saveToEEPROM ( ) ;
_writeLocalParamCache ( ) ; _writeLocalParamCache ( ) ;
} }
_checkInitialLoadComplete ( ) ; _checkInitialLoadComplete ( ) ;
} }
@ -276,19 +276,19 @@ void ParameterLoader::_valueUpdated(const QVariant& value)
{ {
Fact * fact = qobject_cast < Fact * > ( sender ( ) ) ; Fact * fact = qobject_cast < Fact * > ( sender ( ) ) ;
Q_ASSERT ( fact ) ; Q_ASSERT ( fact ) ;
int componentId = fact - > componentId ( ) ; int componentId = fact - > componentId ( ) ;
QString name = fact - > name ( ) ; QString name = fact - > name ( ) ;
_dataMutex . lock ( ) ; _dataMutex . lock ( ) ;
Q_ASSERT ( _waitingWriteParamNameMap . contains ( componentId ) ) ; Q_ASSERT ( _waitingWriteParamNameMap . contains ( componentId ) ) ;
_waitingWriteParamNameMap [ componentId ] . remove ( name ) ; // Remove any old entry
_waitingWriteParamNameMap [ componentId ] . remove ( name ) ; // Remove any old entry
_waitingWriteParamNameMap [ componentId ] [ name ] = 0 ; // Add new entry and set retry count
_waitingWriteParamNameMap [ componentId ] [ name ] = 0 ; // Add new entry and set retry count
_waitingParamTimeoutTimer . start ( ) ; _waitingParamTimeoutTimer . start ( ) ;
_dataMutex . unlock ( ) ; _dataMutex . unlock ( ) ;
_writeParameterRaw ( componentId , fact - > name ( ) , value ) ; _writeParameterRaw ( componentId , fact - > name ( ) , value ) ;
qCDebug ( ParameterLoaderLog ) < < " Set parameter (componentId: " < < componentId < < " name: " < < name < < value < < " ) " ; qCDebug ( ParameterLoaderLog ) < < " Set parameter (componentId: " < < componentId < < " name: " < < name < < value < < " ) " ;
} }
@ -296,7 +296,7 @@ void ParameterLoader::_valueUpdated(const QVariant& value)
void ParameterLoader : : refreshAllParameters ( void ) void ParameterLoader : : refreshAllParameters ( void )
{ {
_dataMutex . lock ( ) ; _dataMutex . lock ( ) ;
// Reset index wait lists
// Reset index wait lists
foreach ( int componentId , _paramCountMap . keys ( ) ) { foreach ( int componentId , _paramCountMap . keys ( ) ) {
// Add/Update all indices to the wait list, parameter index is 0-based
// Add/Update all indices to the wait list, parameter index is 0-based
@ -305,16 +305,16 @@ void ParameterLoader::refreshAllParameters(void)
_waitingReadParamIndexMap [ componentId ] [ waitingIndex ] = 0 ; _waitingReadParamIndexMap [ componentId ] [ waitingIndex ] = 0 ;
} }
} }
_dataMutex . unlock ( ) ; _dataMutex . unlock ( ) ;
MAVLinkProtocol * mavlink = qgcApp ( ) - > toolbox ( ) - > mavlinkProtocol ( ) ; MAVLinkProtocol * mavlink = qgcApp ( ) - > toolbox ( ) - > mavlinkProtocol ( ) ;
Q_ASSERT ( mavlink ) ; Q_ASSERT ( mavlink ) ;
mavlink_message_t msg ; mavlink_message_t msg ;
mavlink_msg_param_request_list_pack ( mavlink - > getSystemId ( ) , mavlink - > getComponentId ( ) , & msg , _vehicle - > id ( ) , MAV_COMP_ID_ALL ) ; mavlink_msg_param_request_list_pack ( mavlink - > getSystemId ( ) , mavlink - > getComponentId ( ) , & msg , _vehicle - > id ( ) , MAV_COMP_ID_ALL ) ;
_vehicle - > sendMessage ( msg ) ; _vehicle - > sendMessage ( msg ) ;
qCDebug ( ParameterLoaderLog ) < < " Request to refresh all parameters " ; qCDebug ( ParameterLoaderLog ) < < " Request to refresh all parameters " ;
} }
@ -324,7 +324,7 @@ void ParameterLoader::_determineDefaultComponentId(void)
// We don't have a default component id yet. That means the plugin can't provide
// We don't have a default component id yet. That means the plugin can't provide
// the param to trigger off of. Instead we use the most prominent component id in
// the param to trigger off of. Instead we use the most prominent component id in
// the set of parameters. Better than nothing!
// the set of parameters. Better than nothing!
_defaultComponentId = - 1 ; _defaultComponentId = - 1 ;
foreach ( int componentId , _mapParameterName2Variant . keys ( ) ) { foreach ( int componentId , _mapParameterName2Variant . keys ( ) ) {
if ( _mapParameterName2Variant [ componentId ] . count ( ) > _defaultComponentId ) { if ( _mapParameterName2Variant [ componentId ] . count ( ) > _defaultComponentId ) {
@ -342,7 +342,7 @@ int ParameterLoader::_actualComponentId(int componentId)
componentId = _defaultComponentId ; componentId = _defaultComponentId ;
Q_ASSERT ( componentId ! = FactSystem : : defaultComponentId ) ; Q_ASSERT ( componentId ! = FactSystem : : defaultComponentId ) ;
} }
return componentId ; return componentId ;
} }
@ -350,17 +350,17 @@ void ParameterLoader::refreshParameter(int componentId, const QString& name)
{ {
componentId = _actualComponentId ( componentId ) ; componentId = _actualComponentId ( componentId ) ;
qCDebug ( ParameterLoaderLog ) < < " refreshParameter (component id: " < < componentId < < " name: " < < name < < " ) " ; qCDebug ( ParameterLoaderLog ) < < " refreshParameter (component id: " < < componentId < < " name: " < < name < < " ) " ;
_dataMutex . lock ( ) ; _dataMutex . lock ( ) ;
Q_ASSERT ( _waitingReadParamNameMap . contains ( componentId ) ) ; Q_ASSERT ( _waitingReadParamNameMap . contains ( componentId ) ) ;
if ( _waitingReadParamNameMap . contains ( componentId ) ) { if ( _waitingReadParamNameMap . contains ( componentId ) ) {
_waitingReadParamNameMap [ componentId ] . remove ( name ) ; // Remove old wait entry if there
_waitingReadParamNameMap [ componentId ] . remove ( name ) ; // Remove old wait entry if there
_waitingReadParamNameMap [ componentId ] [ name ] = 0 ; // Add new wait entry and update retry count
_waitingReadParamNameMap [ componentId ] [ name ] = 0 ; // Add new wait entry and update retry count
emit restartWaitingParamTimer ( ) ; emit restartWaitingParamTimer ( ) ;
} }
_dataMutex . unlock ( ) ; _dataMutex . unlock ( ) ;
_readParameterRaw ( componentId , name , - 1 ) ; _readParameterRaw ( componentId , name , - 1 ) ;
@ -381,7 +381,7 @@ void ParameterLoader::refreshParametersPrefix(int componentId, const QString& na
bool ParameterLoader : : parameterExists ( int componentId , const QString & name ) bool ParameterLoader : : parameterExists ( int componentId , const QString & name )
{ {
bool ret = false ; bool ret = false ;
componentId = _actualComponentId ( componentId ) ; componentId = _actualComponentId ( componentId ) ;
if ( _mapParameterName2Variant . contains ( componentId ) ) { if ( _mapParameterName2Variant . contains ( componentId ) ) {
ret = _mapParameterName2Variant [ componentId ] . contains ( name ) ; ret = _mapParameterName2Variant [ componentId ] . contains ( name ) ;
@ -393,24 +393,24 @@ bool ParameterLoader::parameterExists(int componentId, const QString& name)
Fact * ParameterLoader : : getFact ( int componentId , const QString & name ) Fact * ParameterLoader : : getFact ( int componentId , const QString & name )
{ {
componentId = _actualComponentId ( componentId ) ; componentId = _actualComponentId ( componentId ) ;
if ( ! _mapParameterName2Variant . contains ( componentId ) | | ! _mapParameterName2Variant [ componentId ] . contains ( name ) ) { if ( ! _mapParameterName2Variant . contains ( componentId ) | | ! _mapParameterName2Variant [ componentId ] . contains ( name ) ) {
qgcApp ( ) - > reportMissingParameter ( componentId , name ) ; qgcApp ( ) - > reportMissingParameter ( componentId , name ) ;
return & _defaultFact ; return & _defaultFact ;
} }
return _mapParameterName2Variant [ componentId ] [ name ] . value < Fact * > ( ) ; return _mapParameterName2Variant [ componentId ] [ name ] . value < Fact * > ( ) ;
} }
QStringList ParameterLoader : : parameterNames ( int componentId ) QStringList ParameterLoader : : parameterNames ( int componentId )
{ {
QStringList names ; QStringList names ;
foreach ( QString paramName , _mapParameterName2Variant [ _actualComponentId ( componentId ) ] . keys ( ) ) { foreach ( QString paramName , _mapParameterName2Variant [ _actualComponentId ( componentId ) ] . keys ( ) ) {
names < < paramName ; names < < paramName ;
} }
return names ; return names ;
} }
void ParameterLoader : : _setupGroupMap ( void ) void ParameterLoader : : _setupGroupMap ( void )
@ -433,9 +433,9 @@ void ParameterLoader::_waitingParamTimeout(void)
bool paramsRequested = false ; bool paramsRequested = false ;
const int maxBatchSize = 10 ; const int maxBatchSize = 10 ;
int batchCount = 0 ; int batchCount = 0 ;
// We timed out waiting for some parameters from the initial set. Re-request those.
// We timed out waiting for some parameters from the initial set. Re-request those.
batchCount = 0 ; batchCount = 0 ;
foreach ( int componentId , _waitingReadParamIndexMap . keys ( ) ) { foreach ( int componentId , _waitingReadParamIndexMap . keys ( ) ) {
foreach ( int paramIndex , _waitingReadParamIndexMap [ componentId ] . keys ( ) ) { foreach ( int paramIndex , _waitingReadParamIndexMap [ componentId ] . keys ( ) ) {
@ -450,7 +450,7 @@ void ParameterLoader::_waitingParamTimeout(void)
paramsRequested = true ; paramsRequested = true ;
_readParameterRaw ( componentId , " " , paramIndex ) ; _readParameterRaw ( componentId , " " , paramIndex ) ;
qCDebug ( ParameterLoaderLog ) < < " Read re-request for (componentId: " < < componentId < < " paramIndex: " < < paramIndex < < " retryCount: " < < _waitingReadParamIndexMap [ componentId ] [ paramIndex ] < < " ) " ; qCDebug ( ParameterLoaderLog ) < < " Read re-request for (componentId: " < < componentId < < " paramIndex: " < < paramIndex < < " retryCount: " < < _waitingReadParamIndexMap [ componentId ] [ paramIndex ] < < " ) " ;
if ( + + batchCount > maxBatchSize ) { if ( + + batchCount > maxBatchSize ) {
goto Out ; goto Out ;
} }
@ -459,7 +459,7 @@ void ParameterLoader::_waitingParamTimeout(void)
} }
// We need to check for initial load complete here as well, since it could complete on a max retry failure
// We need to check for initial load complete here as well, since it could complete on a max retry failure
_checkInitialLoadComplete ( ) ; _checkInitialLoadComplete ( ) ;
if ( ! paramsRequested ) { if ( ! paramsRequested ) {
foreach ( int componentId , _waitingWriteParamNameMap . keys ( ) ) { foreach ( int componentId , _waitingWriteParamNameMap . keys ( ) ) {
foreach ( QString paramName , _waitingWriteParamNameMap [ componentId ] . keys ( ) ) { foreach ( QString paramName , _waitingWriteParamNameMap [ componentId ] . keys ( ) ) {
@ -467,14 +467,14 @@ void ParameterLoader::_waitingParamTimeout(void)
_waitingWriteParamNameMap [ componentId ] [ paramName ] + + ; // Bump retry count
_waitingWriteParamNameMap [ componentId ] [ paramName ] + + ; // Bump retry count
_writeParameterRaw ( componentId , paramName , _autopilot - > getFact ( FactSystem : : ParameterProvider , componentId , paramName ) - > value ( ) ) ; _writeParameterRaw ( componentId , paramName , _autopilot - > getFact ( FactSystem : : ParameterProvider , componentId , paramName ) - > value ( ) ) ;
qCDebug ( ParameterLoaderLog ) < < " Write resend for (componentId: " < < componentId < < " paramName: " < < paramName < < " retryCount: " < < _waitingWriteParamNameMap [ componentId ] [ paramName ] < < " ) " ; qCDebug ( ParameterLoaderLog ) < < " Write resend for (componentId: " < < componentId < < " paramName: " < < paramName < < " retryCount: " < < _waitingWriteParamNameMap [ componentId ] [ paramName ] < < " ) " ;
if ( + + batchCount > maxBatchSize ) { if ( + + batchCount > maxBatchSize ) {
goto Out ; goto Out ;
} }
} }
} }
} }
if ( ! paramsRequested ) { if ( ! paramsRequested ) {
foreach ( int componentId , _waitingReadParamNameMap . keys ( ) ) { foreach ( int componentId , _waitingReadParamNameMap . keys ( ) ) {
foreach ( QString paramName , _waitingReadParamNameMap [ componentId ] . keys ( ) ) { foreach ( QString paramName , _waitingReadParamNameMap [ componentId ] . keys ( ) ) {
@ -482,14 +482,14 @@ void ParameterLoader::_waitingParamTimeout(void)
_waitingReadParamNameMap [ componentId ] [ paramName ] + + ; // Bump retry count
_waitingReadParamNameMap [ componentId ] [ paramName ] + + ; // Bump retry count
_readParameterRaw ( componentId , paramName , - 1 ) ; _readParameterRaw ( componentId , paramName , - 1 ) ;
qCDebug ( ParameterLoaderLog ) < < " Read re-request for (componentId: " < < componentId < < " paramName: " < < paramName < < " retryCount: " < < _waitingReadParamNameMap [ componentId ] [ paramName ] < < " ) " ; qCDebug ( ParameterLoaderLog ) < < " Read re-request for (componentId: " < < componentId < < " paramName: " < < paramName < < " retryCount: " < < _waitingReadParamNameMap [ componentId ] [ paramName ] < < " ) " ;
if ( + + batchCount > maxBatchSize ) { if ( + + batchCount > maxBatchSize ) {
goto Out ; goto Out ;
} }
} }
} }
} }
Out : Out :
if ( paramsRequested ) { if ( paramsRequested ) {
_waitingParamTimeoutTimer . start ( ) ; _waitingParamTimeoutTimer . start ( ) ;
@ -529,50 +529,50 @@ void ParameterLoader::_writeParameterRaw(int componentId, const QString& paramNa
{ {
mavlink_param_set_t p ; mavlink_param_set_t p ;
mavlink_param_union_t union_value ; mavlink_param_union_t union_value ;
FactMetaData : : ValueType_t factType = _autopilot - > getFact ( FactSystem : : ParameterProvider , componentId , paramName ) - > type ( ) ; FactMetaData : : ValueType_t factType = _autopilot - > getFact ( FactSystem : : ParameterProvider , componentId , paramName ) - > type ( ) ;
p . param_type = _factTypeToMavType ( factType ) ; p . param_type = _factTypeToMavType ( factType ) ;
switch ( factType ) { switch ( factType ) {
case FactMetaData : : valueTypeUint8 : case FactMetaData : : valueTypeUint8 :
union_value . param_uint8 = ( uint8_t ) value . toUInt ( ) ; union_value . param_uint8 = ( uint8_t ) value . toUInt ( ) ;
break ; break ;
case FactMetaData : : valueTypeInt8 : case FactMetaData : : valueTypeInt8 :
union_value . param_int8 = ( int8_t ) value . toInt ( ) ; union_value . param_int8 = ( int8_t ) value . toInt ( ) ;
break ; break ;
case FactMetaData : : valueTypeUint16 : case FactMetaData : : valueTypeUint16 :
union_value . param_uint16 = ( uint16_t ) value . toUInt ( ) ; union_value . param_uint16 = ( uint16_t ) value . toUInt ( ) ;
break ; break ;
case FactMetaData : : valueTypeInt16 : case FactMetaData : : valueTypeInt16 :
union_value . param_int16 = ( int16_t ) value . toInt ( ) ; union_value . param_int16 = ( int16_t ) value . toInt ( ) ;
break ; break ;
case FactMetaData : : valueTypeUint32 : case FactMetaData : : valueTypeUint32 :
union_value . param_uint32 = ( uint32_t ) value . toUInt ( ) ; union_value . param_uint32 = ( uint32_t ) value . toUInt ( ) ;
break ; break ;
case FactMetaData : : valueTypeFloat : case FactMetaData : : valueTypeFloat :
union_value . param_float = value . toFloat ( ) ; union_value . param_float = value . toFloat ( ) ;
break ; break ;
default : default :
qCritical ( ) < < " Unsupported fact type " < < factType ; qCritical ( ) < < " Unsupported fact type " < < factType ;
// fall through
// fall through
case FactMetaData : : valueTypeInt32 : case FactMetaData : : valueTypeInt32 :
union_value . param_int32 = ( int32_t ) value . toInt ( ) ; union_value . param_int32 = ( int32_t ) value . toInt ( ) ;
break ; break ;
} }
p . param_value = union_value . param_float ; p . param_value = union_value . param_float ;
p . target_system = ( uint8_t ) _vehicle - > id ( ) ; p . target_system = ( uint8_t ) _vehicle - > id ( ) ;
p . target_component = ( uint8_t ) componentId ; p . target_component = ( uint8_t ) componentId ;
strncpy ( p . param_id , paramName . toStdString ( ) . c_str ( ) , sizeof ( p . param_id ) ) ; strncpy ( p . param_id , paramName . toStdString ( ) . c_str ( ) , sizeof ( p . param_id ) ) ;
mavlink_message_t msg ; mavlink_message_t msg ;
mavlink_msg_param_set_encode ( _mavlink - > getSystemId ( ) , _mavlink - > getComponentId ( ) , & msg , & p ) ; mavlink_msg_param_set_encode ( _mavlink - > getSystemId ( ) , _mavlink - > getComponentId ( ) , & msg , & p ) ;
_vehicle - > sendMessage ( msg ) ; _vehicle - > sendMessage ( msg ) ;
@ -658,7 +658,7 @@ QString ParameterLoader::readParametersFromStream(QTextStream& stream)
{ {
QString errors ; QString errors ;
bool userWarned = false ; bool userWarned = false ;
while ( ! stream . atEnd ( ) ) { while ( ! stream . atEnd ( ) ) {
QString line = stream . readLine ( ) ; QString line = stream . readLine ( ) ;
if ( ! line . startsWith ( " # " ) ) { if ( ! line . startsWith ( " # " ) ) {
@ -675,13 +675,13 @@ QString ParameterLoader::readParametersFromStream(QTextStream& stream)
if ( button = = QGCMessageBox : : Cancel ) { if ( button = = QGCMessageBox : : Cancel ) {
return QString ( ) ; return QString ( ) ;
} }
}
}
int componentId = wpParams . at ( 1 ) . toInt ( ) ; int componentId = wpParams . at ( 1 ) . toInt ( ) ;
QString paramName = wpParams . at ( 2 ) ; QString paramName = wpParams . at ( 2 ) ;
QString valStr = wpParams . at ( 3 ) ; QString valStr = wpParams . at ( 3 ) ;
uint mavType = wpParams . at ( 4 ) . toUInt ( ) ; uint mavType = wpParams . at ( 4 ) . toUInt ( ) ;
if ( ! _autopilot - > factExists ( FactSystem : : ParameterProvider , componentId , paramName ) ) { if ( ! _autopilot - > factExists ( FactSystem : : ParameterProvider , componentId , paramName ) ) {
QString error ; QString error ;
error = QString ( " Skipped parameter %1:%2 - does not exist on this vehicle \n " ) . arg ( componentId ) . arg ( paramName ) ; error = QString ( " Skipped parameter %1:%2 - does not exist on this vehicle \n " ) . arg ( componentId ) . arg ( paramName ) ;
@ -689,7 +689,7 @@ QString ParameterLoader::readParametersFromStream(QTextStream& stream)
qCDebug ( ParameterLoaderLog ) < < error ; qCDebug ( ParameterLoaderLog ) < < error ;
continue ; continue ;
} }
Fact * fact = _autopilot - > getFact ( FactSystem : : ParameterProvider , componentId , paramName ) ; Fact * fact = _autopilot - > getFact ( FactSystem : : ParameterProvider , componentId , paramName ) ;
if ( fact - > type ( ) ! = _mavTypeToFactType ( ( MAV_PARAM_TYPE ) mavType ) ) { if ( fact - > type ( ) ! = _mavTypeToFactType ( ( MAV_PARAM_TYPE ) mavType ) ) {
QString error ; QString error ;
@ -698,13 +698,13 @@ QString ParameterLoader::readParametersFromStream(QTextStream& stream)
qCDebug ( ParameterLoaderLog ) < < error ; qCDebug ( ParameterLoaderLog ) < < error ;
continue ; continue ;
} }
qCDebug ( ParameterLoaderLog ) < < " Updating parameter " < < componentId < < paramName < < valStr ; qCDebug ( ParameterLoaderLog ) < < " Updating parameter " < < componentId < < paramName < < valStr ;
fact - > setValue ( valStr ) ; fact - > setValue ( valStr ) ;
} }
} }
} }
return errors ; return errors ;
} }
@ -718,11 +718,11 @@ void ParameterLoader::writeParametersToStream(QTextStream &stream)
foreach ( QString paramName , _mapParameterName2Variant [ componentId ] . keys ( ) ) { foreach ( QString paramName , _mapParameterName2Variant [ componentId ] . keys ( ) ) {
Fact * fact = _mapParameterName2Variant [ componentId ] [ paramName ] . value < Fact * > ( ) ; Fact * fact = _mapParameterName2Variant [ componentId ] [ paramName ] . value < Fact * > ( ) ;
Q_ASSERT ( fact ) ; Q_ASSERT ( fact ) ;
stream < < _vehicle - > id ( ) < < " \t " < < componentId < < " \t " < < paramName < < " \t " < < fact - > valueString ( ) < < " \t " < < QString ( " %1 " ) . arg ( _factTypeToMavType ( fact - > type ( ) ) ) < < " \n " ; stream < < _vehicle - > id ( ) < < " \t " < < componentId < < " \t " < < paramName < < " \t " < < fact - > valueString ( ) < < " \t " < < QString ( " %1 " ) . arg ( _factTypeToMavType ( fact - > type ( ) ) ) < < " \n " ;
} }
} }
stream . flush ( ) ; stream . flush ( ) ;
} }
@ -731,26 +731,26 @@ MAV_PARAM_TYPE ParameterLoader::_factTypeToMavType(FactMetaData::ValueType_t fac
switch ( factType ) { switch ( factType ) {
case FactMetaData : : valueTypeUint8 : case FactMetaData : : valueTypeUint8 :
return MAV_PARAM_TYPE_UINT8 ; return MAV_PARAM_TYPE_UINT8 ;
case FactMetaData : : valueTypeInt8 : case FactMetaData : : valueTypeInt8 :
return MAV_PARAM_TYPE_INT8 ; return MAV_PARAM_TYPE_INT8 ;
case FactMetaData : : valueTypeUint16 : case FactMetaData : : valueTypeUint16 :
return MAV_PARAM_TYPE_UINT16 ; return MAV_PARAM_TYPE_UINT16 ;
case FactMetaData : : valueTypeInt16 : case FactMetaData : : valueTypeInt16 :
return MAV_PARAM_TYPE_INT16 ; return MAV_PARAM_TYPE_INT16 ;
case FactMetaData : : valueTypeUint32 : case FactMetaData : : valueTypeUint32 :
return MAV_PARAM_TYPE_UINT32 ; return MAV_PARAM_TYPE_UINT32 ;
case FactMetaData : : valueTypeFloat : case FactMetaData : : valueTypeFloat :
return MAV_PARAM_TYPE_REAL32 ; return MAV_PARAM_TYPE_REAL32 ;
default : default :
qWarning ( ) < < " Unsupported fact type " < < factType ; qWarning ( ) < < " Unsupported fact type " < < factType ;
// fall through
// fall through
case FactMetaData : : valueTypeInt32 : case FactMetaData : : valueTypeInt32 :
return MAV_PARAM_TYPE_INT32 ; return MAV_PARAM_TYPE_INT32 ;
} }
@ -761,26 +761,26 @@ FactMetaData::ValueType_t ParameterLoader::_mavTypeToFactType(MAV_PARAM_TYPE mav
switch ( mavType ) { switch ( mavType ) {
case MAV_PARAM_TYPE_UINT8 : case MAV_PARAM_TYPE_UINT8 :
return FactMetaData : : valueTypeUint8 ; return FactMetaData : : valueTypeUint8 ;
case MAV_PARAM_TYPE_INT8 : case MAV_PARAM_TYPE_INT8 :
return FactMetaData : : valueTypeInt8 ; return FactMetaData : : valueTypeInt8 ;
case MAV_PARAM_TYPE_UINT16 : case MAV_PARAM_TYPE_UINT16 :
return FactMetaData : : valueTypeUint16 ; return FactMetaData : : valueTypeUint16 ;
case MAV_PARAM_TYPE_INT16 : case MAV_PARAM_TYPE_INT16 :
return FactMetaData : : valueTypeInt16 ; return FactMetaData : : valueTypeInt16 ;
case MAV_PARAM_TYPE_UINT32 : case MAV_PARAM_TYPE_UINT32 :
return FactMetaData : : valueTypeUint32 ; return FactMetaData : : valueTypeUint32 ;
case MAV_PARAM_TYPE_REAL32 : case MAV_PARAM_TYPE_REAL32 :
return FactMetaData : : valueTypeFloat ; return FactMetaData : : valueTypeFloat ;
default : default :
qWarning ( ) < < " Unsupported mav param type " < < mavType ; qWarning ( ) < < " Unsupported mav param type " < < mavType ;
// fall through
// fall through
case MAV_PARAM_TYPE_INT32 : case MAV_PARAM_TYPE_INT32 :
return FactMetaData : : valueTypeInt32 ; return FactMetaData : : valueTypeInt32 ;
} }
@ -797,18 +797,18 @@ void ParameterLoader::_checkInitialLoadComplete(void)
if ( _initialLoadComplete ) { if ( _initialLoadComplete ) {
return ; return ;
} }
foreach ( int componentId , _waitingReadParamIndexMap . keys ( ) ) { foreach ( int componentId , _waitingReadParamIndexMap . keys ( ) ) {
if ( _waitingReadParamIndexMap [ componentId ] . count ( ) ) { if ( _waitingReadParamIndexMap [ componentId ] . count ( ) ) {
// We are still waiting on some parameters, not done yet
// We are still waiting on some parameters, not done yet
return ; return ;
} }
} }
// We aren't waiting for any more initial parameter updates, initial parameter loading is complete
// We aren't waiting for any more initial parameter updates, initial parameter loading is complete
_initialLoadComplete = true ; _initialLoadComplete = true ;
// Check for load failures
// Check for load failures
QString indexList ; QString indexList ;
bool initialLoadFailures = false ; bool initialLoadFailures = false ;
@ -822,20 +822,20 @@ void ParameterLoader::_checkInitialLoadComplete(void)
qCDebug ( ParameterLoaderLog ) < < " Gave up on initial load after max retries (componentId: " < < componentId < < " paramIndex: " < < paramIndex < < " ) " ; qCDebug ( ParameterLoaderLog ) < < " Gave up on initial load after max retries (componentId: " < < componentId < < " paramIndex: " < < paramIndex < < " ) " ;
} }
} }
// Check for any errors during vehicle boot
// Check for any errors during vehicle boot
UASMessageHandler * msgHandler = qgcApp ( ) - > toolbox ( ) - > uasMessageHandler ( ) ; UASMessageHandler * msgHandler = qgcApp ( ) - > toolbox ( ) - > uasMessageHandler ( ) ;
if ( msgHandler - > getErrorCountTotal ( ) ) { if ( msgHandler - > getErrorCountTotal ( ) ) {
QString errors ; QString errors ;
bool firstError = true ; bool firstError = true ;
bool errorsFound = false ; bool errorsFound = false ;
msgHandler - > lockAccess ( ) ; msgHandler - > lockAccess ( ) ;
foreach ( UASMessage * msg , msgHandler - > messages ( ) ) { foreach ( UASMessage * msg , msgHandler - > messages ( ) ) {
if ( msg - > severityIsError ( ) ) { if ( msg - > severityIsError ( ) ) {
if ( ! firstError ) { if ( ! firstError ) {
errors + = " \n " ; errors + = " <br> " ;
} }
errors + = " - " ; errors + = " - " ;
errors + = msg - > getText ( ) ; errors + = msg - > getText ( ) ;
@ -845,15 +845,15 @@ void ParameterLoader::_checkInitialLoadComplete(void)
} }
msgHandler - > showErrorsInToolbar ( ) ; msgHandler - > showErrorsInToolbar ( ) ;
msgHandler - > unlockAccess ( ) ; msgHandler - > unlockAccess ( ) ;
if ( errorsFound ) { if ( errorsFound ) {
QString errorMsg = QString ( " Errors were detected during vehicle startup. You should resolve these prior to flight. \n %1 " ) . arg ( errors ) ; QString errorMsg = QString ( " <b>Critical safety issue detected:</b><br> %1" ) . arg ( errors ) ;
qgcApp ( ) - > showToolBarMessage ( errorMsg ) ; qgcApp ( ) - > showToolBarMessage ( errorMsg ) ;
} }
} }
// Warn of parameter load failure
// Warn of parameter load failure
if ( initialLoadFailures ) { if ( initialLoadFailures ) {
QGCMessageBox : : critical ( " Parameter Load Failure " , QGCMessageBox : : critical ( " Parameter Load Failure " ,
" QGroundControl was unable to retrieve the full set of parameters from the vehicle. " " QGroundControl was unable to retrieve the full set of parameters from the vehicle. "
Gus Grubba
10 years ago
