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;
}
