rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-21 11:54:15 +01:00
Code Issues Releases Activity

Merge branch 'amorale/fixoutputglitchonsave' into next

Browse Source
This commit is contained in:
a*morale 2012-06-26 01:09:18 +02:00
commit 5a1efef3fe
1 changed files with 33 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -67,7 +67,8 @@ static xTaskHandle taskHandle;
static float lastResult[MAX_MIX_ACTUATORS]={0,0,0,0,0,0,0,0}; static float lastResult[MAX_MIX_ACTUATORS]={0,0,0,0,0,0,0,0};
static float lastFilteredResult[MAX_MIX_ACTUATORS]={0,0,0,0,0,0,0,0}; static float lastFilteredResult[MAX_MIX_ACTUATORS]={0,0,0,0,0,0,0,0};
static float filterAccumulator[MAX_MIX_ACTUATORS]={0,0,0,0,0,0,0,0}; static float filterAccumulator[MAX_MIX_ACTUATORS]={0,0,0,0,0,0,0,0};
// used to inform the actuator thread that actuator update rate is changed
static uint8_t updateRateChanged = 0;
// Private functions // Private functions
static void actuatorTask(void* parameters); static void actuatorTask(void* parameters);
@ -76,10 +77,12 @@ static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutr
static void setFailsafe(); static void setFailsafe();
static float MixerCurve(const float throttle, const float* curve, uint8_t elements); static float MixerCurve(const float throttle, const float* curve, uint8_t elements);
static bool set_channel(uint8_t mixer_channel, uint16_t value); static bool set_channel(uint8_t mixer_channel, uint16_t value);
static void change_update_rate();
float ProcessMixer(const int index, const float curve1, const float curve2, float ProcessMixer(const int index, const float curve1, const float curve2,
MixerSettingsData* mixerSettings, ActuatorDesiredData* desired, MixerSettingsData* mixerSettings, ActuatorDesiredData* desired,
const float period); const float period);
static uint16_t lastChannelUpdateFreq[ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM] = {0,0,0,0};
//this structure is equivalent to the UAVObjects for one mixer. //this structure is equivalent to the UAVObjects for one mixer.
typedef struct { typedef struct {
uint8_t type; uint8_t type;
@ -157,9 +160,8 @@ static void actuatorTask(void* parameters)
int16_t ChannelMax[ACTUATORCOMMAND_CHANNEL_NUMELEM]; int16_t ChannelMax[ACTUATORCOMMAND_CHANNEL_NUMELEM];
int16_t ChannelMin[ACTUATORCOMMAND_CHANNEL_NUMELEM]; int16_t ChannelMin[ACTUATORCOMMAND_CHANNEL_NUMELEM];
int16_t ChannelNeutral[ACTUATORCOMMAND_CHANNEL_NUMELEM]; int16_t ChannelNeutral[ACTUATORCOMMAND_CHANNEL_NUMELEM];
uint16_t ChannelUpdateFreq[ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM];
ActuatorSettingsChannelUpdateFreqGet(ChannelUpdateFreq); change_update_rate();
PIOS_Servo_SetHz(&ChannelUpdateFreq[0], ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM);
float * status = (float *)&mixerStatus; //access status objects as an array of floats float * status = (float *)&mixerStatus; //access status objects as an array of floats
@ -179,6 +181,12 @@ static void actuatorTask(void* parameters)
continue; continue;
} }
if(updateRateChanged!=0)
{
change_update_rate();
updateRateChanged=0;
}
// Check how long since last update // Check how long since last update
thisSysTime = xTaskGetTickCount(); thisSysTime = xTaskGetTickCount();
if(thisSysTime > lastSysTime) // reuse dt in case of wraparound if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
@ -547,10 +555,27 @@ static void setFailsafe()
static void actuator_update_rate(UAVObjEvent * ev) static void actuator_update_rate(UAVObjEvent * ev)
{ {
uint16_t ChannelUpdateFreq[ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM]; uint16_t ChannelUpdateFreq[ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM];
if ( ev->obj == ActuatorSettingsHandle() ) { // ActuatoSettings are not changed
if ( ev->obj != ActuatorSettingsHandle() )
return;
ActuatorSettingsChannelUpdateFreqGet(ChannelUpdateFreq); ActuatorSettingsChannelUpdateFreqGet(ChannelUpdateFreq);
// check if the any rate setting is changed
if (lastChannelUpdateFreq[0]!=0 && memcmp(&lastChannelUpdateFreq[0], &ChannelUpdateFreq[0], sizeof(int16_t) * ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM) ==0)
return;
// signal to the actuator task that ChannelUpdateFreq are changed
updateRateChanged = 1;
}
/**
* @brief Change the update rates according to the ActuatorSettingsChannelUpdateFreq.
*/
static void change_update_rate()
{
uint16_t ChannelUpdateFreq[ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM];
// save the new rates
ActuatorSettingsChannelUpdateFreqGet(ChannelUpdateFreq);
memcpy(lastChannelUpdateFreq, ChannelUpdateFreq, sizeof(int16_t) * ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM);
PIOS_Servo_SetHz(&ChannelUpdateFreq[0], ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM); PIOS_Servo_SetHz(&ChannelUpdateFreq[0], ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM);
}
} }
#if defined(ARCH_POSIX) || defined(ARCH_WIN32) #if defined(ARCH_POSIX) || defined(ARCH_WIN32)