rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-29 14:52:12 +01:00
Code Issues Releases Activity

OP-423 add set/get uavobjects functions to access objects fields separately.

Browse Source
This commit is contained in:
Mathieu Rondonneau 2011-06-02 21:18:34 -07:00
parent 5957612870
commit 31a06a5ae7
6 changed files with 252 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
flight/Modules/Actuator/actuator.c
View File

@ -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);
}
}

4
flight/UAVObjects/inc/uavobjectmanager.h
View File

@ -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);

3
flight/UAVObjects/inc/uavobjecttemplate.h
View File

@ -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
/**

115
flight/UAVObjects/uavobjectmanager.c
View File

@ -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

5
flight/UAVObjects/uavobjecttemplate.c
View File

@ -104,6 +104,11 @@ UAVObjHandle $(NAME)Handle()
return handle;
}
/**
* Get/Set object Functions
*/
$(SETGETFIELDS)
/**
* @}
*/

90
ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp
View File

@ -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) {