void buzzer_handler(unsigned int vbat) //needs to be triggered at 100 Hz { unsigned int buz_active=0; static unsigned short error_cnt_mag_fs; static unsigned short error_cnt_mag_inc; static unsigned short error_cnt_compass; unsigned int buz_priority=0; static unsigned short buz_cnt=0; static unsigned int bat_div=5; static int bat_cnt=0, bat_warning=0; static char bat_warning_enabled=0; unsigned char i; if(++buz_cnt>=BUZZ_INTERVAL) buz_cnt=0; //battery warning if(++bat_cnt==100){ bat_cnt=0; bat_div=(ALARM_battery_warning_voltage_high-ALARM_battery_warning_voltage_low)/100; } if(vbat<ALARM_battery_warning_voltage_low) vbat=ALARM_battery_warning_voltage_low; if(vbat<ALARM_battery_warning_voltage_high) //decide if it's really an empty battery { if(bat_warning<ControllerCyclesPerSecond/5) bat_warning++; else bat_warning_enabled=1; } else { if(bat_warning>10) bat_warning-=2; else { bat_warning_enabled=0; buz_active&=~BU_BATTERY; } } if(bat_warning_enabled) { if(bat_cnt>((vbat-ALARM_battery_warning_voltage_low)/bat_div)) buz_active|=BU_BATTERY; //Beeper on else buz_active&=~BU_BATTERY; //Beeper off buz_priority|=BU_BATTERY; } else { buz_active&=~BU_BATTERY; buz_priority&=~BU_BATTERY; } if(buzzer_warnings&BUZZER_WARNING_GPS_BEEP) { if(((GPS_Data.status&0xFF)!=3)&&(LL_1khz_attitude_data.RC_data[5]>200)) //no lock and in GPS mode { buz_priority|=BU_GPS_BEEP; if(buz_cnt<5) buz_active|=BU_GPS_BEEP; else buz_active&=~BU_GPS_BEEP; } else { buz_active&=~BU_GPS_BEEP; buz_priority&=~BU_GPS_BEEP; } } //gyro error if((LL_1khz_attitude_data.flightMode&FM_CALIBRATION_ERROR_GYROS)&&(SYSTEM_initialized)) { buz_priority|=BU_ERROR_GYRO; if(buz_cnt<155) buz_active|=BU_ERROR_GYRO; else if(buz_cnt<160) buz_active&=~BU_ERROR_GYRO; else if(buz_cnt<165) buz_active|=BU_ERROR_GYRO; else buz_active&=~BU_ERROR_GYRO; } else { buz_priority&=~BU_ERROR_GYRO; buz_active&=~BU_ERROR_GYRO; } //ACC error if((LL_1khz_attitude_data.flightMode&FM_CALIBRATION_ERROR_ACC)&&(SYSTEM_initialized)) { buz_priority|=BU_ERROR_ACC; if(buz_cnt<145) buz_active|=BU_ERROR_ACC; else if(buz_cnt<150) buz_active&=~BU_ERROR_ACC; else if(buz_cnt<155) buz_active|=BU_ERROR_ACC; else if(buz_cnt<160) buz_active&=~BU_ERROR_ACC; else if(buz_cnt<165) buz_active|=BU_ERROR_ACC; else buz_active&=~BU_ERROR_ACC; } else { buz_priority&=~BU_ERROR_ACC; buz_active&=~BU_ERROR_ACC; } //ADC error if((LL_1khz_attitude_data.flightMode&FM_ADC_STARTUP_ERROR)&&(SYSTEM_initialized)) { buz_priority|=BU_ERROR_ADC; if(buz_cnt<135) buz_active|=BU_ERROR_ADC; else if(buz_cnt<140) buz_active&=~BU_ERROR_ADC; else if(buz_cnt<145) buz_active|=BU_ERROR_ADC; else if(buz_cnt<150) buz_active&=~BU_ERROR_ADC; else if(buz_cnt<155) buz_active|=BU_ERROR_ADC; else if(buz_cnt<160) buz_active&=~BU_ERROR_ADC; else if(buz_cnt<165) buz_active|=BU_ERROR_ADC; else buz_active&=~BU_ERROR_ADC; } else { buz_priority&=~BU_ERROR_ADC; buz_active&=~BU_ERROR_ADC; } //compass failure: warn 3 seconds only if((LL_1khz_attitude_data.flightMode&FM_COMPASS_FAILURE)&&(SYSTEM_initialized)&&(error_cnt_compass++<400)) { buz_priority|=BU_COMPASS_FAILURE; if(buz_cnt%100<5) buz_active|=BU_COMPASS_FAILURE; else if(buz_cnt%100<10) buz_active&=~BU_COMPASS_FAILURE; else if(buz_cnt%100<15) buz_active|=BU_COMPASS_FAILURE; else if(buz_cnt%100<20) buz_active&=~BU_COMPASS_FAILURE; else if(buz_cnt%100<25) buz_active|=BU_COMPASS_FAILURE; else if(buz_cnt%100<30) buz_active&=~BU_COMPASS_FAILURE; else if(buz_cnt%100<35) buz_active|=BU_COMPASS_FAILURE; else if(buz_cnt%100<40) buz_active&=~BU_COMPASS_FAILURE; else if(buz_cnt%100<45) buz_active|=BU_COMPASS_FAILURE; else if(buz_cnt%100<50) buz_active&=~BU_COMPASS_FAILURE; else buz_active&=~BU_COMPASS_FAILURE; } else { buz_priority&=~BU_COMPASS_FAILURE; buz_active&=~BU_COMPASS_FAILURE; } //mag fieldstrength warning: warn 3 times only if((LL_1khz_attitude_data.flightMode&FM_MAG_FIELD_STRENGTH_ERROR)&&(SYSTEM_initialized)&&(error_cnt_mag_fs++<400)) { buz_priority|=BU_WARNING_MAG_FS; if(buz_cnt%100<5) buz_active|=BU_WARNING_MAG_FS; else if(buz_cnt%100<10) buz_active&=~BU_WARNING_MAG_FS; else if(buz_cnt%100<15) buz_active|=BU_WARNING_MAG_FS; else if(buz_cnt%100<20) buz_active&=~BU_WARNING_MAG_FS; else if(buz_cnt%100<25) buz_active|=BU_WARNING_MAG_FS; else if(buz_cnt%100<30) buz_active&=~BU_WARNING_MAG_FS; else buz_active&=~BU_WARNING_MAG_FS; } else { buz_priority&=~BU_WARNING_MAG_FS; buz_active&=~BU_WARNING_MAG_FS; } //mag inclination warning: warn 3 times only if((LL_1khz_attitude_data.flightMode&FM_MAG_INCLINATION_ERROR)&&(SYSTEM_initialized)&&(error_cnt_mag_inc++<400)) { buz_priority|=BU_WARNING_MAG_INC; if(buz_cnt%100<5) buz_active|=BU_WARNING_MAG_INC; else if(buz_cnt%100<10) buz_active&=~BU_WARNING_MAG_INC; else if(buz_cnt%100<15) buz_active|=BU_WARNING_MAG_INC; else if(buz_cnt%100<20) buz_active&=~BU_WARNING_MAG_INC; else if(buz_cnt%100<25) buz_active|=BU_WARNING_MAG_INC; else if(buz_cnt%100<30) buz_active&=~BU_WARNING_MAG_INC; else if(buz_cnt%100<35) buz_active|=BU_WARNING_MAG_INC; else if(buz_cnt%100<40) buz_active&=~BU_WARNING_MAG_INC; else buz_active&=~BU_WARNING_MAG_INC; } else { buz_priority&=~BU_WARNING_MAG_INC; buz_active&=~BU_WARNING_MAG_INC; } if(buzzer_warnings&BUZZER_WARNING_INIT_BEEP) { if(!SYSTEM_initialized) { buz_priority|=BU_INIT; #ifndef MATLAB if(buz_cnt%100<5){buz_active|=BU_INIT; I2C1_setRGBLed(0, 0, 255);} else if(buz_cnt%100<10) {buz_active&=~BU_INIT; I2C1_setRGBLed(0, 0, 0);} else if(buz_cnt%100<15) {buz_active|=BU_INIT; I2C1_setRGBLed(0, 0, 255);} #if ((__BUILD_CONFIG==0x00) || (__BUILD_CONFIG==0x02)) else if(buz_cnt%100<30) {buz_active&=~BU_INIT; I2C1_setRGBLed(0, 0, 0);} else if(buz_cnt%100<35) {buz_active|=BU_INIT; I2C1_setRGBLed(0, 0, 255);} #endif else {buz_active&=~BU_INIT; I2C1_setRGBLed(0, 0, 0);} #else if(buz_cnt%100<5){buz_active|=BU_INIT; I2C1_setRGBLed(0, 0, 255);} else if(buz_cnt%100<20) {buz_active&=~BU_INIT; I2C1_setRGBLed(0, 0, 0);} else if(buz_cnt%100<25) {buz_active|=BU_INIT; I2C1_setRGBLed(0, 0, 255);} else if(buz_cnt%100<30) {buz_active&=~BU_INIT; I2C1_setRGBLed(0, 0, 0);} else if(buz_cnt%100<35) {buz_active|=BU_INIT; I2C1_setRGBLed(0, 0, 255);} else {buz_active&=~BU_INIT; I2C1_setRGBLed(0, 0, 0);} #endif } else { buz_active&=~BU_INIT; buz_priority&=~BU_INIT; } } //buzzer control for(i=0;i<BUZZ_NR_OF_WARNINGS; i++) { if(buz_priority&(1<<i)) { buz_active&=(1<<i); i=BUZZ_NR_OF_WARNINGS; } } if(buz_active) buzzer(ON); else buzzer(OFF); }
/********************************************************** MAIN **********************************************************/ int main (void) { static int vbat1; //battery_voltage (lowpass-filtered) unsigned int TimerT1, TimerT2; init(); buzzer(OFF); LL_write_init(); //initialize AscTec Firefly LED fin on I2C1 (not necessary on AscTec Hummingbird or Pelican) I2C1Init(); I2C1_setRGBLed(255,0,0); /* Initialize I2C controller. by Xun */ I2CInit(I2CMASTER); ADC0triggerSampling(1<<VOLTAGE_1); //activate ADC sampling generateBuildInfo(); HL_Status.up_time=0; LED(1,ON); ACISDK(); //AscTec Communication Interface: publish variables, set callbacks, etc. //update parameters stored by ACI: //... PTU_init(); //initialize camera PanTiltUnit #ifdef MATLAB //ee_read((unsigned int*)&matlab_params); //read params from eeprom onboard_matlab_initialize(); //initialize matlab code #endif while(1) { if(mainloop_trigger) { TimerT1 = T0TC; if(GPS_timeout<ControllerCyclesPerSecond) GPS_timeout++; else if(GPS_timeout==ControllerCyclesPerSecond) { GPS_timeout=ControllerCyclesPerSecond+1; GPS_Data.status=0; GPS_Data.numSV=0; } //battery monitoring ADC0getSamplingResults(0xFF,adcChannelValues); vbat1=(vbat1*14+(adcChannelValues[VOLTAGE_1]*9872/579))/15; //voltage in mV HL_Status.battery_voltage_1=vbat1; mainloop_cnt++; if(!(mainloop_cnt%10)) buzzer_handler(HL_Status.battery_voltage_1); if(mainloop_trigger) mainloop_trigger--; mainloop(); // CPU Usage calculation TimerT2 = T0TC; if (mainloop_trigger) { HL_Status.cpu_load = 1000; mainloop_overflows++; } else if (TimerT2 < TimerT1) HL_Status.cpu_load = (T0MR0 - TimerT1 + TimerT2)*1000/T0MR0; // load = "timer cycles" / "timer cycles per controller cycle" * 1000 else HL_Status.cpu_load = (TimerT2 - TimerT1)*1000/T0MR0; // load = "timer cycles" / "timer cycles per controller cycle" * 1000 } } return 0; }