00001 /* 00002 * buzzer.c 00003 * 00004 * Created on: 14.09.2011 00005 * Author: daniel 00006 */ 00007 00008 #include "buzzer.h" 00009 #include "main.h" 00010 #include "hardware.h" 00011 #include "system.h" 00012 #include "LL_HL_comm.h" 00013 #include "gpsmath.h" 00014 #include "LPC214x.h" 00015 00016 //Buzzer active defines 00017 #define BU_INIT 0x01 00018 #define BU_ERROR_GYRO 0x02 00019 #define BU_ERROR_ACC 0x04 00020 #define BU_ERROR_ADC 0x08 00021 #define BU_BATTERY 0x10 00022 #define BU_COMPASS_FAILURE 0x20 00023 #define BU_WARNING_MAG_FS 0x40 00024 #define BU_WARNING_MAG_INC 0x80 00025 #define BU_GPS_BEEP 0x100 00026 00027 #define BUZZ_LENGTH 5 //50 ms 00028 #define BUZZ_PAUSE 5 //50 ms 00029 #define BUZZ_INTERVAL 200 //2 s 00030 #define BUZZ_MAG_WARNING_TIMEOUT 500 //mag warning for 5 seconds only 00031 #define BUZZ_NR_OF_WARNINGS 9 //total number of different buzzer signals (see BU_ defines above) 00032 00033 00034 void buzzer_handler(int battery_voltage, int warning_voltage) //needs to be triggered at 100 Hz 00035 { 00036 unsigned int buz_active=0; 00037 static unsigned short error_cnt_mag_fs; 00038 static unsigned short error_cnt_mag_inc; 00039 static unsigned short error_cnt_compass; 00040 00041 unsigned int buz_priority=0; 00042 static unsigned short buz_cnt=0; 00043 00044 static int bat_cnt=0, bat_warning=0; 00045 static char bat_warning_enabled=0; 00046 00047 if(++buz_cnt>=BUZZ_INTERVAL) buz_cnt=0; 00048 00049 //battery warning 00050 if(++bat_cnt==100) bat_cnt=0; 00051 00052 if(battery_voltage<warning_voltage) //decide if it's really an empty battery 00053 { 00054 if(bat_warning<ControllerCyclesPerSecond/5) bat_warning++; 00055 else bat_warning_enabled=1; 00056 } 00057 else 00058 { 00059 if(bat_warning>10) bat_warning-=2; 00060 else 00061 { 00062 bat_warning_enabled=0; 00063 buz_active&=~BU_BATTERY; 00064 } 00065 } 00066 if(bat_warning_enabled) 00067 { 00068 if(bat_cnt > ((1000 - warning_voltage + battery_voltage))/10) buz_active|=BU_BATTERY; //Beeper on 00069 else buz_active&=~BU_BATTERY; //Beeper off 00070 buz_priority|=BU_BATTERY; 00071 } 00072 else 00073 { 00074 buz_active&=~BU_BATTERY; 00075 buz_priority&=~BU_BATTERY; 00076 } 00077 00078 #ifdef GPS_BEEP 00079 if(((GPS_Data.status&0xFF)!=3)&&(LL_1khz_attitude_data.RC_data[5]>200)) //no lock and in GPS mode 00080 { 00081 buz_priority|=BU_GPS_BEEP; 00082 if(buz_cnt<5) buz_active|=BU_GPS_BEEP; 00083 else buz_active&=~BU_GPS_BEEP; 00084 } 00085 else 00086 { 00087 buz_active&=~BU_GPS_BEEP; 00088 buz_priority&=~BU_GPS_BEEP; 00089 } 00090 #endif 00091 00092 #ifdef ERROR_BEEP 00093 //gyro error 00094 if((LL_1khz_attitude_data.flightMode&FM_CALIBRATION_ERROR_GYROS)&&(SYSTEM_initialized)) 00095 { 00096 buz_priority|=BU_ERROR_GYRO; 00097 if(buz_cnt<155) buz_active|=BU_ERROR_GYRO; 00098 else if(buz_cnt<160) buz_active&=~BU_ERROR_GYRO; 00099 else if(buz_cnt<165) buz_active|=BU_ERROR_GYRO; 00100 else buz_active&=~BU_ERROR_GYRO; 00101 } 00102 else 00103 { 00104 buz_priority&=~BU_ERROR_GYRO; 00105 buz_active&=~BU_ERROR_GYRO; 00106 } 00107 00108 //ACC error 00109 if((LL_1khz_attitude_data.flightMode&FM_CALIBRATION_ERROR_ACC)&&(SYSTEM_initialized)) 00110 { 00111 buz_priority|=BU_ERROR_ACC; 00112 if(buz_cnt<145) buz_active|=BU_ERROR_ACC; 00113 else if(buz_cnt<150) buz_active&=~BU_ERROR_ACC; 00114 else if(buz_cnt<155) buz_active|=BU_ERROR_ACC; 00115 else if(buz_cnt<160) buz_active&=~BU_ERROR_ACC; 00116 else if(buz_cnt<165) buz_active|=BU_ERROR_ACC; 00117 else buz_active&=~BU_ERROR_ACC; 00118 } 00119 else 00120 { 00121 buz_priority&=~BU_ERROR_ACC; 00122 buz_active&=~BU_ERROR_ACC; 00123 } 00124 00125 //ADC error 00126 if((LL_1khz_attitude_data.flightMode&FM_ADC_STARTUP_ERROR)&&(SYSTEM_initialized)) 00127 { 00128 buz_priority|=BU_ERROR_ADC; 00129 if(buz_cnt<135) buz_active|=BU_ERROR_ADC; 00130 else if(buz_cnt<140) buz_active&=~BU_ERROR_ADC; 00131 else if(buz_cnt<145) buz_active|=BU_ERROR_ADC; 00132 else if(buz_cnt<150) buz_active&=~BU_ERROR_ADC; 00133 else if(buz_cnt<155) buz_active|=BU_ERROR_ADC; 00134 else if(buz_cnt<160) buz_active&=~BU_ERROR_ADC; 00135 else if(buz_cnt<165) buz_active|=BU_ERROR_ADC; 00136 else buz_active&=~BU_ERROR_ADC; 00137 } 00138 else 00139 { 00140 buz_priority&=~BU_ERROR_ADC; 00141 buz_active&=~BU_ERROR_ADC; 00142 } 00143 00144 //compass failure: warn 3 seconds only 00145 if((LL_1khz_attitude_data.flightMode&FM_COMPASS_FAILURE)&&(SYSTEM_initialized)&&(error_cnt_compass++<400)) 00146 { 00147 buz_priority|=BU_COMPASS_FAILURE; 00148 if(buz_cnt%100<5) buz_active|=BU_COMPASS_FAILURE; 00149 else if(buz_cnt%100<10) buz_active&=~BU_COMPASS_FAILURE; 00150 else if(buz_cnt%100<15) buz_active|=BU_COMPASS_FAILURE; 00151 else if(buz_cnt%100<20) buz_active&=~BU_COMPASS_FAILURE; 00152 else if(buz_cnt%100<25) buz_active|=BU_COMPASS_FAILURE; 00153 else if(buz_cnt%100<30) buz_active&=~BU_COMPASS_FAILURE; 00154 else if(buz_cnt%100<35) buz_active|=BU_COMPASS_FAILURE; 00155 else if(buz_cnt%100<40) buz_active&=~BU_COMPASS_FAILURE; 00156 else if(buz_cnt%100<45) buz_active|=BU_COMPASS_FAILURE; 00157 else if(buz_cnt%100<50) buz_active&=~BU_COMPASS_FAILURE; 00158 else buz_active&=~BU_COMPASS_FAILURE; 00159 } 00160 else 00161 { 00162 buz_priority&=~BU_COMPASS_FAILURE; 00163 buz_active&=~BU_COMPASS_FAILURE; 00164 } 00165 00166 //mag fieldstrength warning: warn 3 times only 00167 if((LL_1khz_attitude_data.flightMode&FM_MAG_FIELD_STRENGTH_ERROR)&&(SYSTEM_initialized)&&(error_cnt_mag_fs++<400)) 00168 { 00169 buz_priority|=BU_WARNING_MAG_FS; 00170 if(buz_cnt%100<5) buz_active|=BU_WARNING_MAG_FS; 00171 else if(buz_cnt%100<10) buz_active&=~BU_WARNING_MAG_FS; 00172 else if(buz_cnt%100<15) buz_active|=BU_WARNING_MAG_FS; 00173 else if(buz_cnt%100<20) buz_active&=~BU_WARNING_MAG_FS; 00174 else if(buz_cnt%100<25) buz_active|=BU_WARNING_MAG_FS; 00175 else if(buz_cnt%100<30) buz_active&=~BU_WARNING_MAG_FS; 00176 else buz_active&=~BU_WARNING_MAG_FS; 00177 } 00178 else 00179 { 00180 buz_priority&=~BU_WARNING_MAG_FS; 00181 buz_active&=~BU_WARNING_MAG_FS; 00182 } 00183 00184 //mag inclination warning: warn 3 times only 00185 if((LL_1khz_attitude_data.flightMode&FM_MAG_INCLINATION_ERROR)&&(SYSTEM_initialized)&&(error_cnt_mag_inc++<400)) 00186 { 00187 buz_priority|=BU_WARNING_MAG_INC; 00188 if(buz_cnt%100<5) buz_active|=BU_WARNING_MAG_INC; 00189 else if(buz_cnt%100<10) buz_active&=~BU_WARNING_MAG_INC; 00190 else if(buz_cnt%100<15) buz_active|=BU_WARNING_MAG_INC; 00191 else if(buz_cnt%100<20) buz_active&=~BU_WARNING_MAG_INC; 00192 else if(buz_cnt%100<25) buz_active|=BU_WARNING_MAG_INC; 00193 else if(buz_cnt%100<30) buz_active&=~BU_WARNING_MAG_INC; 00194 else if(buz_cnt%100<35) buz_active|=BU_WARNING_MAG_INC; 00195 else if(buz_cnt%100<40) buz_active&=~BU_WARNING_MAG_INC; 00196 else buz_active&=~BU_WARNING_MAG_INC; 00197 } 00198 else 00199 { 00200 buz_priority&=~BU_WARNING_MAG_INC; 00201 buz_active&=~BU_WARNING_MAG_INC; 00202 } 00203 00204 00205 #endif 00206 00207 #ifdef INIT_BEEP 00208 00209 if(!SYSTEM_initialized) 00210 { 00211 buz_priority|=BU_INIT; 00212 if(buz_cnt%100<5) buz_active|=BU_INIT; 00213 else if(buz_cnt%100<10) buz_active&=~BU_INIT; 00214 else if(buz_cnt%100<15) buz_active|=BU_INIT; 00215 else buz_active&=~BU_INIT; 00216 } 00217 else 00218 { 00219 buz_active&=~BU_INIT; 00220 buz_priority&=~BU_INIT; 00221 } 00222 #endif 00223 00224 //buzzer control 00225 for(unsigned char i=0;i<BUZZ_NR_OF_WARNINGS; i++) 00226 { 00227 if(buz_priority&(1<<i)) 00228 { 00229 buz_active&=(1<<i); 00230 i=BUZZ_NR_OF_WARNINGS; 00231 } 00232 } 00233 00234 if(buz_active) buzzer(ON); 00235 else buzzer(OFF); 00236 } 00237 00238 00239 void buzzer(unsigned char offon) 00240 { 00241 00242 if(offon) //beeper on 00243 { 00244 IOSET1 = (1<<17); 00245 } 00246 else 00247 { 00248 IOCLR1 = (1<<17); 00249 } 00250 } 00251