From 31a06a5ae77cc4dbe5133faf90e306ee12467863 Mon Sep 17 00:00:00 2001 From: Mathieu Rondonneau Date: Thu, 2 Jun 2011 21:18:34 -0700 Subject: [PATCH] OP-423 add set/get uavobjects functions to access objects fields separately. --- flight/Modules/Actuator/actuator.c | 63 +++++----- flight/UAVObjects/inc/uavobjectmanager.h | 4 + flight/UAVObjects/inc/uavobjecttemplate.h | 3 + flight/UAVObjects/uavobjectmanager.c | 115 ++++++++++++++++++ flight/UAVObjects/uavobjecttemplate.c | 5 + .../flight/uavobjectgeneratorflight.cpp | 90 ++++++++++++++ 6 files changed, 252 insertions(+), 28 deletions(-) diff --git a/flight/Modules/Actuator/actuator.c b/flight/Modules/Actuator/actuator.c index b2f143adf..05ad47e7a 100644 --- a/flight/Modules/Actuator/actuator.c +++ b/flight/Modules/Actuator/actuator.c @@ -128,17 +128,20 @@ static void actuatorTask(void* parameters) portTickType lastSysTime; portTickType thisSysTime; float dT = 0.0f; - ActuatorCommandData command; - ActuatorSettingsData settings; - SystemSettingsData sysSettings; + ActuatorCommandData command; MixerSettingsData mixerSettings; ActuatorDesiredData desired; MixerStatusData mixerStatus; FlightStatusData flightStatus; - - ActuatorSettingsGet(&settings); - PIOS_Servo_SetHz(&settings.ChannelUpdateFreq[0], ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM); + + uint8_t MotorsSpinWhileArmed; + int16_t ChannelMax[ACTUATORCOMMAND_CHANNEL_NUMELEM]; + int16_t ChannelMin[ACTUATORCOMMAND_CHANNEL_NUMELEM]; + int16_t ChannelNeutral[ACTUATORCOMMAND_CHANNEL_NUMELEM]; + uint16_t ChannelUpdateFreq[ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM]; + ActuatorSettingsChannelUpdateFreqGet(ChannelUpdateFreq); + PIOS_Servo_SetHz(&ChannelUpdateFreq[0], ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM); float * status = (float *)&mixerStatus; //access status objects as an array of floats @@ -164,14 +167,16 @@ static void actuatorTask(void* parameters) dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; lastSysTime = thisSysTime; - FlightStatusGet(&flightStatus); - SystemSettingsGet(&sysSettings); MixerStatusGet(&mixerStatus); MixerSettingsGet (&mixerSettings); ActuatorDesiredGet(&desired); ActuatorCommandGet(&command); - ActuatorSettingsGet(&settings); + + ActuatorSettingsMotorsSpinWhileArmedGet(&MotorsSpinWhileArmed); + ActuatorSettingsChannelMaxGet(ChannelMax); + ActuatorSettingsChannelMinGet(ChannelMin); + ActuatorSettingsChannelNeutralGet(ChannelNeutral); int nMixers = 0; Mixer_t * mixers = (Mixer_t *)&mixerSettings.Mixer1Type; @@ -192,7 +197,7 @@ static void actuatorTask(void* parameters) bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED; bool positiveThrottle = desired.Throttle >= 0.00; - bool spinWhileArmed = settings.MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE; + bool spinWhileArmed = MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE; float curve1 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve1); float curve2 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve2); @@ -223,11 +228,11 @@ static void actuatorTask(void* parameters) (status[ct] < 0) ) status[ct] = 0; } - + command.Channel[ct] = scaleChannel(status[ct], - settings.ChannelMax[ct], - settings.ChannelMin[ct], - settings.ChannelNeutral[ct]); + ChannelMax[ct], + ChannelMin[ct], + ChannelNeutral[ct]); } MixerStatusSet(&mixerStatus); @@ -391,11 +396,13 @@ static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutr */ static void setFailsafe() { - ActuatorCommandData command; - ActuatorSettingsData settings; - - ActuatorCommandGet(&command); - ActuatorSettingsGet(&settings); + /* grab only the modules parts that we are going to use */ + int16_t ChannelMin[ACTUATORCOMMAND_CHANNEL_NUMELEM]; + ActuatorSettingsChannelMinGet(ChannelMin); + int16_t ChannelNeutral[ACTUATORCOMMAND_CHANNEL_NUMELEM]; + ActuatorSettingsChannelNeutralGet(ChannelNeutral); + int16_t Channel[ACTUATORCOMMAND_CHANNEL_NUMELEM]; + ActuatorCommandChannelGet(Channel); MixerSettingsData mixerSettings; MixerSettingsGet (&mixerSettings); @@ -407,15 +414,15 @@ static void setFailsafe() if(mixers[n].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) { - command.Channel[n] = settings.ChannelMin[n]; + Channel[n] = ChannelMin[n]; } else if(mixers[n].type == MIXERSETTINGS_MIXER1TYPE_SERVO) { - command.Channel[n] = settings.ChannelNeutral[n]; + Channel[n] = ChannelNeutral[n]; } else { - command.Channel[n] = 0; + Channel[n] = 0; } } @@ -425,11 +432,11 @@ static void setFailsafe() // Update servo outputs for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) { - set_channel(n, command.Channel[n]); + set_channel(n, Channel[n]); } - // Update output object - ActuatorCommandSet(&command); + // Update output object's parts that we changed + ActuatorCommandChannelGet(Channel); } @@ -438,10 +445,10 @@ static void setFailsafe() */ static void actuator_update_rate(UAVObjEvent * ev) { - ActuatorSettingsData settings; + uint16_t ChannelUpdateFreq[ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM]; if ( ev->obj == ActuatorSettingsHandle() ) { - ActuatorSettingsGet(&settings); - PIOS_Servo_SetHz(&settings.ChannelUpdateFreq[0], ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM); + ActuatorSettingsChannelUpdateFreqGet(ChannelUpdateFreq); + PIOS_Servo_SetHz(&ChannelUpdateFreq[0], ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM); } } diff --git a/flight/UAVObjects/inc/uavobjectmanager.h b/flight/UAVObjects/inc/uavobjectmanager.h index e7bb8b9c7..ac5d34b5c 100644 --- a/flight/UAVObjects/inc/uavobjectmanager.h +++ b/flight/UAVObjects/inc/uavobjectmanager.h @@ -153,9 +153,13 @@ int32_t UAVObjSaveMetaobjects(); int32_t UAVObjLoadMetaobjects(); int32_t UAVObjDeleteMetaobjects(); int32_t UAVObjSetData(UAVObjHandle obj, const void* dataIn); +int32_t UAVObjSetDataField(UAVObjHandle obj, const void* dataIn, uint32_t offset, uint32_t size); int32_t UAVObjGetData(UAVObjHandle obj, void* dataOut); +int32_t UAVObjGetDataField(UAVObjHandle obj, void* dataOut, uint32_t offset, uint32_t size); int32_t UAVObjSetInstanceData(UAVObjHandle obj, uint16_t instId, const void* dataIn); +int32_t UAVObjSetInstanceDataField(UAVObjHandle obj, uint16_t instId, const void* dataIn, uint32_t offset, uint32_t size); int32_t UAVObjGetInstanceData(UAVObjHandle obj, uint16_t instId, void* dataOut); +int32_t UAVObjGetInstanceDataField(UAVObjHandle obj, uint16_t instId, void* dataOut, uint32_t offset, uint32_t size); int32_t UAVObjSetMetadata(UAVObjHandle obj, const UAVObjMetadata* dataIn); int32_t UAVObjGetMetadata(UAVObjHandle obj, UAVObjMetadata* dataOut); int8_t UAVObjReadOnly(UAVObjHandle obj); diff --git a/flight/UAVObjects/inc/uavobjecttemplate.h b/flight/UAVObjects/inc/uavobjecttemplate.h index 938c8ebf7..9564d05d2 100644 --- a/flight/UAVObjects/inc/uavobjecttemplate.h +++ b/flight/UAVObjects/inc/uavobjecttemplate.h @@ -82,6 +82,9 @@ int32_t $(NAME)Initialize(); UAVObjHandle $(NAME)Handle(); void $(NAME)SetDefaults(UAVObjHandle obj, uint16_t instId); +// set/Get functions +$(SETGETFIELDSEXTERN) + #endif // $(NAMEUC)_H /** diff --git a/flight/UAVObjects/uavobjectmanager.c b/flight/UAVObjects/uavobjectmanager.c index ef1774628..dc28fe462 100644 --- a/flight/UAVObjects/uavobjectmanager.c +++ b/flight/UAVObjects/uavobjectmanager.c @@ -993,6 +993,17 @@ int32_t UAVObjSetData(UAVObjHandle obj, const void *dataIn) return UAVObjSetInstanceData(obj, 0, dataIn); } +/** + * Set the object data + * \param[in] obj The object handle + * \param[in] dataIn The object's data structure + * \return 0 if success or -1 if failure + */ +int32_t UAVObjSetDataField(UAVObjHandle obj, const void* dataIn, uint32_t offset, uint32_t size) +{ + return UAVObjSetInstanceDataField(obj, 0, dataIn, offset, size); +} + /** * Get the object data * \param[in] obj The object handle @@ -1004,6 +1015,17 @@ int32_t UAVObjGetData(UAVObjHandle obj, void *dataOut) return UAVObjGetInstanceData(obj, 0, dataOut); } +/** + * Get the object data + * \param[in] obj The object handle + * \param[out] dataOut The object's data structure + * \return 0 if success or -1 if failure + */ +int32_t UAVObjGetDataField(UAVObjHandle obj, void* dataOut, uint32_t offset, uint32_t size) +{ + return UAVObjGetInstanceDataField(obj, 0, dataOut, offset, size); +} + /** * Set the data of a specific object instance * \param[in] obj The object handle @@ -1052,6 +1074,60 @@ int32_t UAVObjSetInstanceData(UAVObjHandle obj, uint16_t instId, return 0; } +/** + * Set the data of a specific object instance + * \param[in] obj The object handle + * \param[in] instId The object instance ID + * \param[in] dataIn The object's data structure + * \return 0 if success or -1 if failure + */ +int32_t UAVObjSetInstanceDataField(UAVObjHandle obj, uint16_t instId, const void* dataIn, uint32_t offset, uint32_t size) +{ + ObjectList* objEntry; + ObjectInstList* instEntry; + UAVObjMetadata* mdata; + + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Cast to object info + objEntry = (ObjectList*)obj; + + // Check access level + if ( !objEntry->isMetaobject ) + { + mdata = (UAVObjMetadata*)(objEntry->linkedObj->instances.data); + if ( mdata->access == ACCESS_READONLY ) + { + xSemaphoreGiveRecursive(mutex); + return -1; + } + } + + // Get instance information + instEntry = getInstance(objEntry, instId); + if ( instEntry == NULL ) + { + // Error, unlock and return + xSemaphoreGiveRecursive(mutex); + return -1; + } + + // return if we set too much of what we have + if ( (size + offset) > objEntry->numBytes) + return -1; + + // Set data + memcpy(instEntry->data + offset, dataIn, size); + + // Fire event + sendEvent(objEntry, instId, EV_UPDATED); + + // Unlock + xSemaphoreGiveRecursive(mutex); + return 0; +} + /** * Get the data of a specific object instance * \param[in] obj The object handle @@ -1086,6 +1162,45 @@ int32_t UAVObjGetInstanceData(UAVObjHandle obj, uint16_t instId, return 0; } +/** + * Get the data of a specific object instance + * \param[in] obj The object handle + * \param[in] instId The object instance ID + * \param[out] dataOut The object's data structure + * \return 0 if success or -1 if failure + */ +int32_t UAVObjGetInstanceDataField(UAVObjHandle obj, uint16_t instId, void* dataOut, uint32_t offset, uint32_t size) +{ + ObjectList* objEntry; + ObjectInstList* instEntry; + + // Lock + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + + // Cast to object info + objEntry = (ObjectList*)obj; + + // Get instance information + instEntry = getInstance(objEntry, instId); + if ( instEntry == NULL ) + { + // Error, unlock and return + xSemaphoreGiveRecursive(mutex); + return -1; + } + + // return if we request too much of what we can give + if ( (size + offset) > objEntry->numBytes) + return -1; + + // Set data + memcpy(dataOut, instEntry->data + offset, size); + + // Unlock + xSemaphoreGiveRecursive(mutex); + return 0; +} + /** * Set the object metadata * \param[in] obj The object handle diff --git a/flight/UAVObjects/uavobjecttemplate.c b/flight/UAVObjects/uavobjecttemplate.c index 4ee7cc9b1..3fd1517eb 100644 --- a/flight/UAVObjects/uavobjecttemplate.c +++ b/flight/UAVObjects/uavobjecttemplate.c @@ -104,6 +104,11 @@ UAVObjHandle $(NAME)Handle() return handle; } +/** + * Get/Set object Functions + */ +$(SETGETFIELDS) + /** * @} */ diff --git a/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp b/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp index 400a8fa00..8d3a30b30 100644 --- a/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp +++ b/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp @@ -237,6 +237,96 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo* info) } outCode.replace(QString("$(INITFIELDS)"), initfields); + // Replace the $(SETGETFIELDS) tag + QString setgetfields; + for (int n = 0; n < info->fields.length(); ++n) + { + //if (!info->fields[n]->defaultValues.isEmpty() ) + { + // For non-array fields + if ( info->fields[n]->numElements == 1) + { + + /* Set */ + setgetfields.append( QString("void %2%3Set( %1 *New%3 )\r\n") + .arg( fieldTypeStrC[info->fields[n]->type] ) + .arg( info->name ) + .arg( info->fields[n]->name ) ); + setgetfields.append( QString("{\r\n") ); + setgetfields.append( QString("\tUAVObjSetDataField(%1Handle(), (void*)New%2, offsetof( %1Data, %2), sizeof(%3));\r\n") + .arg( info->name ) + .arg( info->fields[n]->name ) + .arg( fieldTypeStrC[info->fields[n]->type] ) ); + setgetfields.append( QString("}\r\n") ); + + /* GET */ + setgetfields.append( QString("void %2%3Get( %1 *New%3 )\r\n") + .arg( fieldTypeStrC[info->fields[n]->type] ) + .arg( info->name ) + .arg( info->fields[n]->name )); + setgetfields.append( QString("{\r\n") ); + setgetfields.append( QString("\tUAVObjGetDataField(%1Handle(), (void*)New%2, offsetof( %1Data, %2), sizeof(%3));\r\n") + .arg( info->name ) + .arg( info->fields[n]->name ) + .arg( fieldTypeStrC[info->fields[n]->type] ) ); + setgetfields.append( QString("}\r\n") ); + + } + else + { + + /* SET */ + setgetfields.append( QString("void %2%3Set( %1 *New%3 )\r\n") + .arg( fieldTypeStrC[info->fields[n]->type] ) + .arg( info->name ) + .arg( info->fields[n]->name ) ); + setgetfields.append( QString("{\r\n") ); + setgetfields.append( QString("\tUAVObjSetDataField(%1Handle(), (void*)New%2, offsetof( %1Data, %2), %3*sizeof(%4));\r\n") + .arg( info->name ) + .arg( info->fields[n]->name ) + .arg( info->fields[n]->numElements ) + .arg( fieldTypeStrC[info->fields[n]->type] ) ); + setgetfields.append( QString("}\r\n") ); + + /* GET */ + setgetfields.append( QString("void %2%3Get( %1 *New%3 )\r\n") + .arg( fieldTypeStrC[info->fields[n]->type] ) + .arg( info->name ) + .arg( info->fields[n]->name ) ); + setgetfields.append( QString("{\r\n") ); + setgetfields.append( QString("\tUAVObjGetDataField(%1Handle(), (void*)New%2, offsetof( %1Data, %2), %3*sizeof(%4));\r\n") + .arg( info->name ) + .arg( info->fields[n]->name ) + .arg( info->fields[n]->numElements ) + .arg( fieldTypeStrC[info->fields[n]->type] ) ); + setgetfields.append( QString("}\r\n") ); + } + } + } + outCode.replace(QString("$(SETGETFIELDS)"), setgetfields); + + // Replace the $(SETGETFIELDSEXTERN) tag + QString setgetfieldsextern; + for (int n = 0; n < info->fields.length(); ++n) + { + //if (!info->fields[n]->defaultValues.isEmpty() ) + { + + /* SET */ + setgetfieldsextern.append( QString("extern void %2%3Set( %1 *New%3 );\r\n") + .arg( fieldTypeStrC[info->fields[n]->type] ) + .arg( info->name ) + .arg( info->fields[n]->name ) ); + + /* GET */ + setgetfieldsextern.append( QString("extern void %2%3Get( %1 *New%3 );\r\n") + .arg( fieldTypeStrC[info->fields[n]->type] ) + .arg( info->name ) + .arg( info->fields[n]->name ) ); + } + } + outInclude.replace(QString("$(SETGETFIELDSEXTERN)"), setgetfieldsextern); + // Write the flight code bool res = writeFileIfDiffrent( flightOutputPath.absolutePath() + "/" + info->namelc + ".c", outCode ); if (!res) {