rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

OP-699 - refacor ugly buzzer code actuaor module

Browse Source
This commit is contained in:
Corvus Corax 2012-11-05 10:09:43 +01:00
parent 774dde3da5
commit e2ca9cbb45
1 changed files with 98 additions and 111 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -556,6 +556,95 @@ static void setFailsafe(const ActuatorSettingsData * actuatorSettings, const Mix
ActuatorCommandChannelSet(Channel); ActuatorCommandChannelSet(Channel);
} }
/**
* determine buzzer or blink sequence
**/
typedef enum {BUZZ_BUZZER=0,BUZZ_ARMING=1,BUZZ_INFO=2,BUZZ_MAX=3} buzzertype;
static inline bool buzzerState(buzzertype type)
{
// This is for buzzers that take a PWM input
static uint32_t tune[BUZZ_MAX]={0};
static uint32_t tunestate[BUZZ_MAX]={0};
uint32_t newTune = 0;
if(type==BUZZ_BUZZER)
{
// Decide what tune to play
if (AlarmsGet(SYSTEMALARMS_ALARM_BATTERY) > SYSTEMALARMS_ALARM_WARNING) {
newTune = 0b11110110110000; // pause, short, short, short, long
} else if (AlarmsGet(SYSTEMALARMS_ALARM_GPS) >= SYSTEMALARMS_ALARM_WARNING) {
newTune = 0x80000000; // pause, short
} else {
newTune = 0;
}
} else { // BUZZ_ARMING || BUZZ_INFO
uint8_t arming;
FlightStatusArmedGet(&arming);
//base idle tune
newTune = 0x80000000; // 0b1000...
// Merge the error pattern for InfoLed
if(type==BUZZ_INFO)
{
if (AlarmsGet(SYSTEMALARMS_ALARM_BATTERY) > SYSTEMALARMS_ALARM_WARNING)
{
newTune |= 0b00000000001111111011111110000000;
}
else if(AlarmsGet(SYSTEMALARMS_ALARM_GPS) >= SYSTEMALARMS_ALARM_WARNING)
{
newTune |= 0b00000000000000110110110000000000;
}
}
// fast double blink pattern if armed
if (arming == FLIGHTSTATUS_ARMED_ARMED)
newTune |= 0xA0000000; // 0b101000...
}
// Do we need to change tune?
if (newTune != tune[type]) {
tune[type] = newTune;
// resynchronize all tunes on change, so they stay in sync
for (int i=0;i<BUZZ_MAX;i++) {
tunestate[i] = tune[i];
}
}
// Play tune
bool buzzOn = false;
static portTickType lastSysTime = 0;
portTickType thisSysTime = xTaskGetTickCount();
portTickType dT = 0;
// For now, only look at the battery alarm, because functions like AlarmsHasCritical() can block for some time; to be discussed
if (tune[type]) {
if(thisSysTime > lastSysTime) {
dT = thisSysTime - lastSysTime;
} else {
lastSysTime = 0; // avoid the case where SysTimeMax-lastSysTime <80
}
buzzOn = (tunestate[type]&1);
if (dT > 80) {
// Go to next bit in alarm_seq_state
for (int i=0;i<BUZZ_MAX;i++) {
tunestate[i] >>=1;
if (tunestate[i]==0) // All done, re-start the tune
tunestate[i]=tune[i];
}
lastSysTime = thisSysTime;
}
}
return buzzOn;
}
#if defined(ARCH_POSIX) || defined(ARCH_WIN32) #if defined(ARCH_POSIX) || defined(ARCH_WIN32)
static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSettingsData * actuatorSettings) static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSettingsData * actuatorSettings)
{ {
@ -566,118 +655,17 @@ static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSet
{ {
switch(actuatorSettings->ChannelType[mixer_channel]) { switch(actuatorSettings->ChannelType[mixer_channel]) {
case ACTUATORSETTINGS_CHANNELTYPE_PWMALARMBUZZER: case ACTUATORSETTINGS_CHANNELTYPE_PWMALARMBUZZER:
case ACTUATORSETTINGS_CHANNELTYPE_ARMINGLED:
case ACTUATORSETTINGS_CHANNELTYPE_INFOLED:
{
// This is for buzzers that take a PWM input
static uint32_t currBuzzTune = 0;
static uint32_t currBuzzTuneState;
static uint32_t currArmingTune = 0;
static uint32_t currArmingTuneState;
static uint32_t currInfoTune = 0;
static uint32_t currInfoTuneState;
uint32_t newTune = 0;
if(actuatorSettings->ChannelType[mixer_channel] == ACTUATORSETTINGS_CHANNELTYPE_PWMALARMBUZZER)
{
// Decide what tune to play
if (AlarmsGet(SYSTEMALARMS_ALARM_BATTERY) > SYSTEMALARMS_ALARM_WARNING) {
newTune = 0b11110110110000; // pause, short, short, short, long
} else if (AlarmsGet(SYSTEMALARMS_ALARM_GPS) >= SYSTEMALARMS_ALARM_WARNING) {
newTune = 0x80000000; // pause, short
} else {
newTune = 0;
}
// Do we need to change tune?
if (newTune != currBuzzTune) {
currBuzzTune = newTune;
currBuzzTuneState = currBuzzTune;
}
}
else // ACTUATORSETTINGS_CHANNELTYPE_ARMINGLED || ACTUATORSETTINGS_CHANNELTYPE_INFOLED
{
uint8_t arming;
FlightStatusArmedGet(&arming);
//base idle tune
newTune = 0x80000000; // 0b1000...
// Merge the error pattern for InfoLed
if(actuatorSettings->ChannelType[mixer_channel] == ACTUATORSETTINGS_CHANNELTYPE_INFOLED)
{
if (AlarmsGet(SYSTEMALARMS_ALARM_BATTERY) > SYSTEMALARMS_ALARM_WARNING)
{
newTune |= 0b00000000001111111011111110000000;
}
else if(AlarmsGet(SYSTEMALARMS_ALARM_GPS) >= SYSTEMALARMS_ALARM_WARNING)
{
newTune |= 0b00000000000000110110110000000000;
}
}
// fast double blink pattern if armed
if (arming == FLIGHTSTATUS_ARMED_ARMED)
newTune |= 0xA0000000; // 0b101000...
// Do we need to change tune?
if(actuatorSettings->ChannelType[mixer_channel] == ACTUATORSETTINGS_CHANNELTYPE_ARMINGLED)
{
if (newTune != currArmingTune) {
currArmingTune = newTune;
// note: those are both updated so that Info and Arming are in sync if used simultaneously
currArmingTuneState = currArmingTune;
currInfoTuneState = currInfoTune;
}
}
else
{
if (newTune != currInfoTune) {
currInfoTune = newTune;
currArmingTuneState = currArmingTune;
currInfoTuneState = currInfoTune;
}
}
}
// Play tune
bool buzzOn = false;
static portTickType lastSysTime = 0;
portTickType thisSysTime = xTaskGetTickCount();
portTickType dT = 0;
// For now, only look at the battery alarm, because functions like AlarmsHasCritical() can block for some time; to be discussed
if (currBuzzTune||currArmingTune||currInfoTune) {
if(thisSysTime > lastSysTime)
dT = thisSysTime - lastSysTime;
if(actuatorSettings->ChannelType[mixer_channel] == ACTUATORSETTINGS_CHANNELTYPE_PWMALARMBUZZER)
buzzOn = (currBuzzTuneState&1); // Buzz when the LS bit is 1
else if(actuatorSettings->ChannelType[mixer_channel] == ACTUATORSETTINGS_CHANNELTYPE_ARMINGLED)
buzzOn = (currArmingTuneState&1);
else if(actuatorSettings->ChannelType[mixer_channel] == ACTUATORSETTINGS_CHANNELTYPE_INFOLED)
buzzOn = (currInfoTuneState&1);
if (dT > 80) {
// Go to next bit in alarm_seq_state
currArmingTuneState >>=1;
currInfoTuneState >>= 1;
currBuzzTuneState >>= 1;
if (currBuzzTuneState == 0)
currBuzzTuneState = currBuzzTune; // All done, re-start the tune
if (currArmingTuneState == 0)
currArmingTuneState = currArmingTune;
if (currInfoTuneState == 0)
currInfoTuneState = currInfoTune;
lastSysTime = thisSysTime;
}
}
PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel],
buzzOn?actuatorSettings->ChannelMax[mixer_channel]:actuatorSettings->ChannelMin[mixer_channel]); buzzerState(BUZZ_BUZZER)?actuatorSettings->ChannelMax[mixer_channel]:actuatorSettings->ChannelMin[mixer_channel]);
return true;
case ACTUATORSETTINGS_CHANNELTYPE_ARMINGLED:
PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel],
buzzerState(BUZZ_ARMING)?actuatorSettings->ChannelMax[mixer_channel]:actuatorSettings->ChannelMin[mixer_channel]);
return true;
case ACTUATORSETTINGS_CHANNELTYPE_INFOLED:
PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel],
buzzerState(BUZZ_INFO)?actuatorSettings->ChannelMax[mixer_channel]:actuatorSettings->ChannelMin[mixer_channel]);
return true; return true;
}
case ACTUATORSETTINGS_CHANNELTYPE_PWM: case ACTUATORSETTINGS_CHANNELTYPE_PWM:
PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], value); PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], value);
return true; return true;
@ -686,7 +674,6 @@ static bool set_channel(uint8_t mixer_channel, uint16_t value, const ActuatorSet
return PIOS_SetMKSpeed(actuatorSettings->ChannelAddr[mixer_channel],value); return PIOS_SetMKSpeed(actuatorSettings->ChannelAddr[mixer_channel],value);
case ACTUATORSETTINGS_CHANNELTYPE_ASTEC4: case ACTUATORSETTINGS_CHANNELTYPE_ASTEC4:
return PIOS_SetAstec4Speed(actuatorSettings->ChannelAddr[mixer_channel],value); return PIOS_SetAstec4Speed(actuatorSettings->ChannelAddr[mixer_channel],value);
break;
#endif #endif
default: default:
return false; return false;