uint8_t getActuatorSettingsParamByIndex(uint16_t index, mavlink_param_union_t* param)
{
ActuatorSettingsData settings;
ActuatorSettingsGet(&settings);
switch (index)
{
case 0:
{
param->param_uint32 = settings.FixedWingRoll1;
param->type = MAVLINK_TYPE_UINT32_T;
}
break;
case 1:
{
param->param_uint32 = settings.FixedWingRoll2;
param->type = MAVLINK_TYPE_UINT32_T;
}
break;
default:
{
return MAVLINK_RET_VAL_PARAM_NAME_DOES_NOT_EXIST;
}
break;
}
// Not returned in default case, return true
return MAVLINK_RET_VAL_PARAM_SUCCESS;
}
/**
* @brief Main module task
*/
static void actuatorTask(void* parameters)
{
// UAVObjEvent ev;
portTickType lastSysTime;
ActuatorCommandData command;
ActuatorSettingsData settings;
// Set servo update frequency (done only on start-up)
ActuatorSettingsGet(&settings);
PIOS_Servo_SetHz(settings.ChannelUpdateFreq[0], settings.ChannelUpdateFreq[1]);
// Go to the neutral (failsafe) values until an ActuatorDesired update is received
setFailsafe();
// Main task loop
lastSysTime = xTaskGetTickCount();
while (1)
{
ActuatorCommandGet(&command);
ActuatorSettingsGet(&settings);
if ( RunMixers(&command, &settings) == -1 )
{
AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL);
}
else
{
AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR);
}
// Update output object
ActuatorCommandSet(&command);
// Update in case read only (eg. during servo configuration)
ActuatorCommandGet(&command);
// Update servo outputs
for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n)
{
PIOS_Servo_Set( n, command.Channel[n] );
}
// Wait until next update
vTaskDelayUntil(&lastSysTime, settings.UpdatePeriod / portTICK_RATE_MS );
}
}
uint8_t setActuatorSettingsParamByIndex(uint16_t index, const mavlink_param_union_t* param)
{
ActuatorSettingsData settings;
ActuatorSettingsGet(&settings);
switch (index)
{
case 0:
{
if (param->type == MAVLINK_TYPE_UINT32_T)
{
settings.FixedWingRoll1 = param->param_uint32;
}
else
{
return MAVLINK_RET_VAL_PARAM_TYPE_MISMATCH;
}
}
break;
case 1:
{
if (param->type == MAVLINK_TYPE_UINT32_T)
{
settings.FixedWingRoll2 = param->param_uint32;
}
else
{
return MAVLINK_RET_VAL_PARAM_TYPE_MISMATCH;
}
}
break;
default:
{
return MAVLINK_RET_VAL_PARAM_NAME_DOES_NOT_EXIST;
}
break;
}
// Not returned in default case, try to write (ok == 0) and return result of
// write operation
if (ActuatorSettingsSet(&settings) == 0)
{
return MAVLINK_RET_VAL_PARAM_SUCCESS;
}
else
{
return MAVLINK_RET_VAL_PARAM_WRITE_ERROR;
}
}
/* PWMOutSet(int,int): set output pulse width of an unused output channel */
void SystemPWMOutSet(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
{
int16_t channel = Param[0]->Val->Integer;
int16_t value = Param[1]->Val->Integer;
// check channel range
if ((channel < 0) || (channel >= ACTUATORSETTINGS_CHANNELNEUTRAL_NUMELEM))
return;
// check mixer settings
MixerSettingsData mixerSettings;
MixerSettingsGet(&mixerSettings);
// this structure is equivalent to the UAVObjects for one mixer.
typedef struct {
uint8_t type;
int8_t matrix[5];
} __attribute__((packed)) Mixer_t;
// base pointer to mixer vectors and types
Mixer_t * mixers = (Mixer_t *)&mixerSettings.Mixer1Type;
// the mixer has to be disabled for this channel.
if (mixers[channel].type != MIXERSETTINGS_MIXER1TYPE_DISABLED)
return;
// check actuator settings
ActuatorSettingsData actuatorSettings;
ActuatorSettingsGet(&actuatorSettings);
// the channel type has to be a PWM output.
if (actuatorSettings.ChannelType[channel] != ACTUATORSETTINGS_CHANNELTYPE_PWM)
return;
actuatorSettings.ChannelMin[channel] = value;
actuatorSettings.ChannelMax[channel] = value;
actuatorSettings.ChannelNeutral[channel] = value;
ActuatorSettingsSet(&actuatorSettings);
}
