diff --git a/flight/Modules/Actuator/actuator.c b/flight/Modules/Actuator/actuator.c index 335cc25ec..c96915610 100644 --- a/flight/Modules/Actuator/actuator.c +++ b/flight/Modules/Actuator/actuator.c @@ -556,6 +556,95 @@ static void setFailsafe(const ActuatorSettingsData * actuatorSettings, const Mix 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 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>=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) 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]) { 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], - 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; - } case ACTUATORSETTINGS_CHANNELTYPE_PWM: PIOS_Servo_Set(actuatorSettings->ChannelAddr[mixer_channel], value); 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); case ACTUATORSETTINGS_CHANNELTYPE_ASTEC4: return PIOS_SetAstec4Speed(actuatorSettings->ChannelAddr[mixer_channel],value); - break; #endif default: return false;