int powerkey(void)
{
unsigned char data;
unsigned int ret;
static int keypressed = 0;
ret = Read_bus(&ux400_ftdic, 0x00, POWERKEY_OFFSET, &data, 1);
if((data == 0x1F)||(data == 0x15)||(data == 0x19)||(data == 0xFF)) return 0;
if((data & POWERKEY) == POWERKEY) {
if(keypressed == 0){
keypressed = 1;
buzzer(1);
usleep(BUZZER_DELAY);
buzzer(0);
usleep(BUZZER_DELAY);
buzzer(1);
usleep(BUZZER_DELAY);
buzzer(0);
}
return 1;
}
return 0;
}
// Plays short tune to signal a player loss
void loseMusic() {
buzzer(noteBb);
usleep(500000);
buzzer(noteA);
usleep(500000);
buzzer(noteAb);
usleep(500000);
buzzer(noteG);
usleep(1000000);
buzzer(0);
}
// Plays short tune to signal a player win
void winMusic() {
buzzer(noteA);
usleep(500000);
buzzer(noteBb);
usleep(500000);
buzzer(noteB);
usleep(500000);
buzzer(noteC);
usleep(1000000);
buzzer(0);
}
static int print_doc_msg_func(int pnum, int num_pages)
{
char temp[10];
int key;
if ( pnum == -1 ) /* successful completion */
{
static FCODE msg[] = {"Done -- Press any key"};
buzzer(0);
putstringcenter(7, 0, 80, C_HELP_LINK, msg);
getakey();
return (0);
}
if ( pnum == -2 ) /* aborted */
{
static FCODE msg[] = {"Aborted -- Press any key"};
buzzer(1);
putstringcenter(7, 0, 80, C_HELP_LINK, msg);
getakey();
return (0);
}
if (pnum == 0) /* initialization */
{
static FCODE msg[] = {"Generating FRACTINT.DOC"};
helptitle();
printinstr();
setattr(2, 0, C_HELP_BODY, 80*22);
putstringcenter(1, 0, 80, C_HELP_HDG, msg);
putstring(7, 30, C_HELP_BODY, "Completed:");
movecursor(25,80); /* hide cursor */
}
sprintf(temp, "%d%%", (int)( (100.0 / num_pages) * pnum ) );
putstring(7, 41, C_HELP_LINK, temp);
while ( keypressed() )
{
key = getakey();
if ( key == ESC )
return (0); /* user abort */
}
return (1); /* AOK -- continue */
}
void uTurn()
{
buzzer();
right();
angle_rotate(190);
//_delay_ms(1000);
}
// Closes all files for exit when termination signal it received
void shutDown() {
buzzer(0);
if (pwm != NULL) fclose(pwm);
if (dirDuty != NULL) fclose(dirDuty);
if (dirT != NULL) fclose(dirT);
if (fd_lcd != 0) close(fd_lcd);
if (fd_but != 0) close(fd_but);
}
void lcd(char *str)
{
lcd_wr_command(0x01);
lcd_cursor(1,1);
lcd_string(str);
buzzer();
_delay_ms(1000);
}
static void mwDisarm(void)
{
if (f.ARMED) {
f.ARMED = 0;
// Beep for inform about disarming
buzzer(BUZZER_DISARMING);
// Reset disarm time so that it works next time we arm the board.
if (disarmTime != 0)
disarmTime = 0;
}
}
void countfunc()
{
int requiredcount=20;
while(1)
{if(count<requiredcount)
{
forward();
buzzer();
_delay_ms(500);
break;}
}
stop();
}
static void mwArm(void)
{
if (calibratingG == 0 && f.ACC_CALIBRATED)
{
// TODO: feature(FEATURE_FAILSAFE) && failsafeCnt < 2
// TODO: && ( !feature || ( feature && ( failsafecnt > 2) )
if (!f.ARMED) { // arm now!
f.ARMED = 1;
headFreeModeHold = heading;
// Beep for inform about arming
#ifdef GPS
if (feature(FEATURE_GPS) && f.GPS_FIX && GPS_numSat >= 5)
buzzer(BUZZER_ARMING_GPS_FIX);
else
buzzer(BUZZER_ARMING);
#else
buzzer(BUZZER_ARMING);
#endif
}
} else if (!f.ARMED) {
blinkLED(2, 255, 1);
}
}
void nodeInd()
{
lcd_print(2, 1, 0, 1);
turnDelay();
noNatak();
right();
angle_rotate(90);
////_delay_ms(1000);
buzzer();
right();
angle_rotate(190);
////_delay_ms(1000);
buzzer();
right();
angle_rotate(80);
buzzer();
forward();
//_delay_ms(5000);
}
void LoadOn ( unsigned char number )
{
if ( number == overhead )
{
if ( !( OOneFourth ) )
{
Load &= OMotor_Off ;
OClearScreen( ) ;
Printfunction ( "Upper Motor off" ) ;
delay ( delaynumber ) ;
}
else
{
Load |= OMotor_On ;
buzzer() ;
}
}
else if ( number == ground )
{
if ( !( GOneFourth ) )
{
Load &= GMotor_Off ;
GClearScreen( ) ;
Printfunction ( "Lower Motor off" ) ;
delay ( delaynumber ) ;
}
else
{
Load |= GMotor_On ;
buzzer() ;
}
}
}
void travel(unsigned char nxTerm)
{
forwardJaa();
//swapEncounterdAction
swapMan(CT % 2, nxTerm);
/*
lcd_print(1,11,(CT==1 && (nxTerm == 3 || nxTerm== 4)),1);// ||
lcd_print(1,12,(CT==4 && (nxTerm == 1 || nxTerm== 2)),1);
lcd_print(1,13,(CT==2 && (nxTerm == 3 || nxTerm== 4)),1); // ||
lcd_print(1,14,(CT==3 && (nxTerm == 1 || nxTerm== 2)),1);
*/
if ((CT == 1 && (nxTerm == 2 || nxTerm == 4)) || (CT == 4 && (nxTerm == 1 || nxTerm == 3)))
{
nodeLeft();
forwardJaa();
//_delay_ms(1000);
forwardJaa();
CT = nxTerm; //changed for drop,pickup
swapMan(nxTerm % 2, nxTerm);
if (nxTerm == 1 || nxTerm == 4)
nodeRight();
else
nodeLeft();
}
else if ((CT == 2 && (nxTerm == 3 || nxTerm == 1)) || (CT == 3 && (nxTerm == 4 || nxTerm == 2)))
{
nodeRight();
forwardJaa();
//_delay_ms(500);
forwardJaa();
CT = nxTerm; //changed for drop,pickup
swapMan(nxTerm % 2, nxTerm);
if (nxTerm == 2 || nxTerm == 3)
nodeLeft();
else
nodeRight();
}
else
{
//_delay_ms(1000);
}
CT = nxTerm;
forwardJaa();
stop();
stop();
buzzer();
}
//***************************************************************************
//! \brief main函数
//***************************************************************************
void main(void)
{
WDTR = 0x5A;
STKP |= 0x07;
initial(); // 系统初始化
while(1)
{
WDTR = 0x5A;
time_flag();
timer(); // 时间处理
key(); // 按键处理
led(); // led显示
buzzer(); // 蜂鸣器处理
rec(); // 红外接收
fun();
}
}
static void speechSynthesize(FILE *uart) {
int taajuus;
int userchoice = 0;
char speechSentence[128];
DDRB= 0x0f;
PORTB=0x0f;
PORTB = 14;
lcdPrint("Generate speech");
do {
fflush(uart);
puts("\n\n\n\n\n");
puts("\n********************");
puts("\n* Generate speech *");
puts("\n********************\n");
puts("\n1. Text to speech\n");
puts("\n2. Return to Main Menu\n");
puts("\n*3. Buzzer*\n");
puts("\nEnter your choice: ");
readLine(inbuf, sizeof(inbuf));
if (inbuf[0]) {
puts("\n\n");
userchoice = atoi(inbuf);
switch (userchoice)
{
case 1: puts("\nEnter sentence you want to hear: ");
readLine(speechSentence, sizeof(speechSentence));
printf("\nCan you hear: %s\n", speechSentence);
break;
case 2: puts("\nReturning to Main Menu\n");
puts("\n\n\n\n\n");
break;
case 3: puts("\nInsert normalized frequency value: ");
readLine(speechSentence, sizeof(speechSentence));
taajuus=atoi(speechSentence);
printf("\nBuzzing at %s.\n", speechSentence);
buzzer(taajuus);
break;
default: puts("\nInvalid option selected\n");
}
}
}
while (userchoice != 2);
}
int main(void)
{
const unsigned short MainCycle = 60;
Init(MainCycle);
int i;
unsigned short br;
unsigned short bl;
for(i=0; i<N; i++){
br = ADRead(0); //left
bl = ADRead(1); //right
if(bl<200 && br<200){
step();
}
if (bl>200 && br<200){
LED(1);
Wait(100);
LED(0);
right();
}
if(br>200 && bl<200){
LED(2);
Wait(100);
LED(0);
left();
}
else if(br>200 && bl>200){
forward(0,0);
break;
}
}
buzzer();
return 0;
}
void initialize() {
counter_a = 0;
counter_b = 0;
configure_ports();
configure_timers();
init_step_motor();
init_servodrive();
init_proxi_switch();
init_delays();
sei();
while(INIT_TIME--) {
buzzer(440, 100);
delay_s(1);
}
}
/**********************************************************
MAIN
**********************************************************/
int main (void) {
static int vbat1; //battery_voltage (lowpass-filtered)
init();
buzzer(OFF);
LL_write_init();
PTU_init();
ADC0triggerSampling(1<<VOLTAGE_1); //activate ADC sampling
HL_Status.up_time=0;
LED(1,ON);
while(1)
{
if(mainloop_trigger)
{
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();
}
}
return 0;
}
void pickSort(int armNo, int sortNo)
{
lcd_print(2,10,armNo,1);
lcd_print(2,12,sortNo,1);
buzzer();
_delay_ms(1000);
arm[armNo] = sort[sortNo];
if (((armNo == sortNo) && dir == 0) || ((armNo != sortNo) && dir == 2))
turn();
else
{
if (dir == 1 || dir == 3)
{
if (armNo == 0)
turnLeft();
else turnRight();
}
}
pick(armNo);
sort[sortNo] = -1;
//..printf("Arm %d picked %d from sort[%d]\n", armNo, arm[armNo], sortNo);
lcd("exit picksort");
}
void annexCode(void)
{
static uint32_t calibratedAccTime;
int32_t tmp, tmp2;
int32_t axis, prop1, prop2;
static uint8_t buzzerFreq; // delay between buzzer ring
// vbat shit
static uint8_t vbatTimer = 0;
static int32_t vbatRaw = 0;
static int32_t amperageRaw = 0;
static int64_t mAhdrawnRaw = 0;
static int32_t vbatCycleTime = 0;
// PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value
if (rcData[THROTTLE] < cfg.tpa_breakpoint) {
prop2 = 100;
} else {
if (rcData[THROTTLE] < 2000) {
prop2 = 100 - (uint16_t)cfg.dynThrPID * (rcData[THROTTLE] - cfg.tpa_breakpoint) / (2000 - cfg.tpa_breakpoint);
} else {
prop2 = 100 - cfg.dynThrPID;
}
}
for (axis = 0; axis < 3; axis++) {
tmp = min(abs(rcData[axis] - mcfg.midrc), 500);
if (axis != 2) { // ROLL & PITCH
if (cfg.deadband) {
if (tmp > cfg.deadband) {
tmp -= cfg.deadband;
} else {
tmp = 0;
}
}
tmp2 = tmp / 100;
rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
prop1 = 100 - (uint16_t)cfg.rollPitchRate * tmp / 500;
prop1 = (uint16_t)prop1 * prop2 / 100;
} else { // YAW
if (cfg.yawdeadband) {
if (tmp > cfg.yawdeadband) {
tmp -= cfg.yawdeadband;
} else {
tmp = 0;
}
}
rcCommand[axis] = tmp * -mcfg.yaw_control_direction;
prop1 = 100 - (uint16_t)cfg.yawRate * abs(tmp) / 500;
}
dynP8[axis] = (uint16_t)cfg.P8[axis] * prop1 / 100;
dynI8[axis] = (uint16_t)cfg.I8[axis] * prop1 / 100;
dynD8[axis] = (uint16_t)cfg.D8[axis] * prop1 / 100;
if (rcData[axis] < mcfg.midrc)
rcCommand[axis] = -rcCommand[axis];
}
tmp = constrain(rcData[THROTTLE], mcfg.mincheck, 2000);
tmp = (uint32_t)(tmp - mcfg.mincheck) * 1000 / (2000 - mcfg.mincheck); // [MINCHECK;2000] -> [0;1000]
tmp2 = tmp / 100;
rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
if (f.HEADFREE_MODE) {
float radDiff = (heading - headFreeModeHold) * M_PI / 180.0f;
float cosDiff = cosf(radDiff);
float sinDiff = sinf(radDiff);
int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
rcCommand[PITCH] = rcCommand_PITCH;
}
if (feature(FEATURE_VBAT)) {
vbatCycleTime += cycleTime;
if (!(++vbatTimer % VBATFREQ)) {
vbatRaw -= vbatRaw / 8;
vbatRaw += adcGetChannel(ADC_BATTERY);
vbat = batteryAdcToVoltage(vbatRaw / 8);
if (mcfg.power_adc_channel > 0) {
amperageRaw -= amperageRaw / 8;
amperageRaw += adcGetChannel(ADC_EXTERNAL_CURRENT);
amperage = currentSensorToCentiamps(amperageRaw / 8);
mAhdrawnRaw += (amperage * vbatCycleTime) / 1000;
mAhdrawn = mAhdrawnRaw / (3600 * 100);
vbatCycleTime = 0;
}
}
if ((vbat > batteryWarningVoltage) || (vbat < mcfg.vbatmincellvoltage)) { // VBAT ok, buzzer off
buzzerFreq = 0;
} else
buzzerFreq = 4; // low battery
}
buzzer(buzzerFreq); // external buzzer routine that handles buzzer events globally now
if ((calibratingA > 0 && sensors(SENSOR_ACC)) || (calibratingG > 0)) { // Calibration phasis
LED0_TOGGLE;
} else {
if (f.ACC_CALIBRATED)
LED0_OFF;
if (f.ARMED)
LED0_ON;
#ifndef CJMCU
checkTelemetryState();
#endif
}
#ifdef LEDRING
if (feature(FEATURE_LED_RING)) {
static uint32_t LEDTime;
if ((int32_t)(currentTime - LEDTime) >= 0) {
LEDTime = currentTime + 50000;
ledringState();
}
}
#endif
if ((int32_t)(currentTime - calibratedAccTime) >= 0) {
if (!f.SMALL_ANGLE) {
f.ACC_CALIBRATED = 0; // the multi uses ACC and is not calibrated or is too much inclinated
LED0_TOGGLE;
calibratedAccTime = currentTime + 500000;
} else {
f.ACC_CALIBRATED = 1;
}
}
serialCom();
#ifndef CJMCU
if (!cliMode && feature(FEATURE_TELEMETRY)) {
handleTelemetry();
}
#endif
if (sensors(SENSOR_GPS)) {
static uint32_t GPSLEDTime;
if ((int32_t)(currentTime - GPSLEDTime) >= 0 && (GPS_numSat >= 5)) {
GPSLEDTime = currentTime + 150000;
LED1_TOGGLE;
}
}
// Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
if (gyro.temperature)
gyro.temperature(&telemTemperature1);
else {
// TODO MCU temp
}
}
char handle_soltic(char *s,int timeout)
{
unsigned char temp[18];
portTickType xLastWakeTime;
char i=0;
char status_solt=2;
xLastWakeTime = xTaskGetTickCount();
#ifdef debug
printf("start solt ic card!\r\n");
#endif
do {
i++;
status_solt=PcdRequest(PICC_REQIDL,temp);
vTaskDelayUntil( &xLastWakeTime, ( timeout / portTICK_RATE_MS ) );
}
while((status_solt!=0)&&(i<50));
i=0;
if(0==status_solt) { //
#ifdef debug
printf("PcdRequest OK!\r\n");
#endif
do {
i++;
status_solt=PcdAnticoll(temp);
vTaskDelayUntil( &xLastWakeTime, ( timeout / portTICK_RATE_MS ) );
}
while((status_solt!=0)&&(i<10));
i=0;
if(0==status_solt) { //
#ifdef debug
printf("PcdAnticoll is OK \r\n");
#endif
memcpy(s,temp,4);
buzzer(100);
return 0;
}
else {
#ifdef debug
printf("PcdAnticoll error \r\n");
#endif
buzzer(200);
return 2;
}
} else { //认为是没有刷卡，或者是无效卡
#ifdef debug
printf("solt ic card error!\r\n");
#endif
}
return 1;
}
void sortCheck()
{
//int a1, a2;
int arm0 = 0, arm1 = 1;
if (((ct == 0 || ct == 1) && dir == 2) || ((ct == 2 || ct == 3) && dir == 0))
{
arm0 = 1;
arm1 = 0;
}
if ((sort[ct % 2] == color[ct] || sort[ct % 2] == color[adj]))
{
if (armCount>0)
{
ct = indicator[sort[ct % 2]];
if (arm[arm0] == -1)
pickSort(arm0, ct % 2);
else if (arm[arm1] == -1)
pickSort(arm1, ct % 2);
if (armCount != 1 && term[ct][0] != -1 && term[ct][1] != -1 && sort[ct % 2] == -1)
{
if (arm[arm0] == color[ct])
sortDrop(arm1, ct % 2);
else if (arm[arm1] == color[ct])
sortDrop(arm0, ct % 2);
}
}
else ct = adj;
}
else if (arm[arm0] != color[adj] && arm[arm1] != color[adj])
{
if (armCount == 0 || (armCount == 1 && sort[ct % 2] != -1) || ((armCount == 2 || armCount == 1) && (term[adj][0] == color[adj] || term[adj][0] == -1) && (term[adj][1] == color[adj] || term[adj][1] == -1)))
{
if (armCount>0)
{
if(sort[ct % 2] != -1)
{
if (arm[arm0] == -1)
pickSort(arm0, ct % 2);
else if (arm[arm1] == -1)
pickSort(arm1, ct % 2);
}
}
traverseToSort(ct % 2 + 4, (ct + 1) % 2 + 4);
newSort();
if (ct == 4 || ct == 5)
newSort();
return;
}
else ct = adj;
}
else
{
if (armCount == 0 && ((visited[adj] == 1 && (term[adj][0] != -1 && term[adj][1] != -1)) || visited[adj] == 0) && sort[ct % 2] == -1)
if (arm[arm0] == color[adj])
sortDrop(arm1, ct % 2);
else sortDrop(arm0, ct % 2);
else if (armCount == 1 && (term[adj][0] == -1 || term[adj][1] == -1) && sort[ct % 2] != -1)
if (arm[arm0] == -1)
pickSort(arm0, ct % 2);
else pickSort(arm1, ct % 2);
ct = adj;
}
lcd_print(2,15,ct,1);
buzzer();
_delay_ms(1000);
adj = adjCount(ct);
}
// this code is executed at each loop and won't interfere with control loop if it lasts less than 650 microseconds
void annexCode(void)
{
static uint32_t calibratedAccTime;
uint16_t tmp, tmp2;
static uint8_t buzzerFreq; //delay between buzzer ring
static uint8_t vbatTimer = 0;
uint8_t axis, prop1, prop2;
static uint8_t ind = 0;
uint16_t vbatRaw = 0;
static uint16_t vbatRawArray[8];
uint8_t i;
// PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value
if (rcData[THROTTLE] < BREAKPOINT) {
prop2 = 100;
} else {
if (rcData[THROTTLE] < 2000) {
prop2 = 100 - (uint16_t) cfg.dynThrPID * (rcData[THROTTLE] - BREAKPOINT) / (2000 - BREAKPOINT);
} else {
prop2 = 100 - cfg.dynThrPID;
}
}
for (axis = 0; axis < 3; axis++) {
tmp = min(abs(rcData[axis] - cfg.midrc), 500);
if (axis != 2) { // ROLL & PITCH
if (cfg.deadband) {
if (tmp > cfg.deadband) {
tmp -= cfg.deadband;
} else {
tmp = 0;
}
}
tmp2 = tmp / 100;
rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
prop1 = 100 - (uint16_t) cfg.rollPitchRate * tmp / 500;
prop1 = (uint16_t) prop1 *prop2 / 100;
} else { // YAW
if (cfg.yawdeadband) {
if (tmp > cfg.yawdeadband) {
tmp -= cfg.yawdeadband;
} else {
tmp = 0;
}
}
rcCommand[axis] = tmp;
prop1 = 100 - (uint16_t) cfg.yawRate * tmp / 500;
}
dynP8[axis] = (uint16_t) cfg.P8[axis] * prop1 / 100;
dynD8[axis] = (uint16_t) cfg.D8[axis] * prop1 / 100;
if (rcData[axis] < cfg.midrc)
rcCommand[axis] = -rcCommand[axis];
}
tmp = constrain(rcData[THROTTLE], cfg.mincheck, 2000);
tmp = (uint32_t) (tmp - cfg.mincheck) * 1000 / (2000 - cfg.mincheck); // [MINCHECK;2000] -> [0;1000]
tmp2 = tmp / 100;
rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
if(f.HEADFREE_MODE) {
float radDiff = (heading - headFreeModeHold) * M_PI / 180.0f;
float cosDiff = cosf(radDiff);
float sinDiff = sinf(radDiff);
int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
rcCommand[PITCH] = rcCommand_PITCH;
}
if (feature(FEATURE_VBAT)) {
if (!(++vbatTimer % VBATFREQ)) {
vbatRawArray[(ind++) % 8] = adcGetBattery();
for (i = 0; i < 8; i++)
vbatRaw += vbatRawArray[i];
vbat = batteryAdcToVoltage(vbatRaw / 8);
}
if ((vbat > batteryWarningVoltage) || (vbat < cfg.vbatmincellvoltage)) { // VBAT ok, buzzer off
buzzerFreq = 0;
} else
buzzerFreq = 4; // low battery
}
buzzer(buzzerFreq); // external buzzer routine that handles buzzer events globally now
if ((calibratingA > 0 && sensors(SENSOR_ACC)) || (calibratingG > 0)) { // Calibration phasis
LED0_TOGGLE;
} else {
if (f.ACC_CALIBRATED)
LED0_OFF;
if (f.ARMED)
LED0_ON;
// This will switch to/from 9600 or 115200 baud depending on state. Of course, it should only do it on changes.
if (feature(FEATURE_TELEMETRY))
initTelemetry(f.ARMED);
}
#ifdef LEDRING
if (feature(FEATURE_LED_RING)) {
static uint32_t LEDTime;
if ((int32_t)(currentTime - LEDTime) >= 0) {
LEDTime = currentTime + 50000;
ledringState();
}
}
#endif
if ((int32_t)(currentTime - calibratedAccTime) >= 0) {
if (!f.SMALL_ANGLES_25) {
f.ACC_CALIBRATED = 0; // the multi uses ACC and is not calibrated or is too much inclinated
LED0_TOGGLE;
calibratedAccTime = currentTime + 500000;
} else {
f.ACC_CALIBRATED = 1;
}
}
serialCom();
if (sensors(SENSOR_GPS)) {
static uint32_t GPSLEDTime;
if ((int32_t)(currentTime - GPSLEDTime) >= 0 && (GPS_numSat >= 5)) {
GPSLEDTime = currentTime + 150000;
LED1_TOGGLE;
}
}
// Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
if (gyro.temperature)
gyro.temperature(&telemTemperature1);
else {
// TODO MCU temp
}
}
interrupt void isr_t0()
{
init_timer();
buzzer();
scanner();
}
static void ACC_Common(void)
{
static int32_t a[3];
int axis;
if (calibratingA > 0) {
for (axis = 0; axis < 3; axis++) {
// Reset a[axis] at start of calibration
if (calibratingA == CALIBRATING_ACC_CYCLES)
a[axis] = 0;
// Sum up CALIBRATING_ACC_CYCLES readings
a[axis] += accADC[axis];
// Clear global variables for next reading
accADC[axis] = 0;
mcfg.accZero[axis] = 0;
}
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
if (calibratingA == 1) {
mcfg.accZero[ROLL] = (a[ROLL] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
mcfg.accZero[PITCH] = (a[PITCH] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
mcfg.accZero[YAW] = (a[YAW] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES - acc_1G;
cfg.angleTrim[ROLL] = 0;
cfg.angleTrim[PITCH] = 0;
writeEEPROM(1, true); // write accZero in EEPROM
}
calibratingA--;
}
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
static int32_t b[3];
static int16_t accZero_saved[3] = { 0, 0, 0 };
static int16_t angleTrim_saved[2] = { 0, 0 };
// Saving old zeropoints before measurement
if (InflightcalibratingA == 50) {
accZero_saved[ROLL] = mcfg.accZero[ROLL];
accZero_saved[PITCH] = mcfg.accZero[PITCH];
accZero_saved[YAW] = mcfg.accZero[YAW];
angleTrim_saved[ROLL] = cfg.angleTrim[ROLL];
angleTrim_saved[PITCH] = cfg.angleTrim[PITCH];
}
if (InflightcalibratingA > 0) {
for (axis = 0; axis < 3; axis++) {
// Reset a[axis] at start of calibration
if (InflightcalibratingA == 50)
b[axis] = 0;
// Sum up 50 readings
b[axis] += accADC[axis];
// Clear global variables for next reading
accADC[axis] = 0;
mcfg.accZero[axis] = 0;
}
// all values are measured
if (InflightcalibratingA == 1) {
AccInflightCalibrationActive = false;
AccInflightCalibrationMeasurementDone = true;
buzzer(BUZZER_ACC_CALIBRATION); // buzzer for indicatiing the end of calibration
// recover saved values to maintain current flight behavior until new values are transferred
mcfg.accZero[ROLL] = accZero_saved[ROLL];
mcfg.accZero[PITCH] = accZero_saved[PITCH];
mcfg.accZero[YAW] = accZero_saved[YAW];
cfg.angleTrim[ROLL] = angleTrim_saved[ROLL];
cfg.angleTrim[PITCH] = angleTrim_saved[PITCH];
}
InflightcalibratingA--;
}
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
if (AccInflightCalibrationSavetoEEProm) { // the copter is landed, disarmed and the combo has been done again
AccInflightCalibrationSavetoEEProm = false;
mcfg.accZero[ROLL] = b[ROLL] / 50;
mcfg.accZero[PITCH] = b[PITCH] / 50;
mcfg.accZero[YAW] = b[YAW] / 50 - acc_1G; // for nunchuk 200=1G
cfg.angleTrim[ROLL] = 0;
cfg.angleTrim[PITCH] = 0;
writeEEPROM(1, true); // write accZero in EEPROM
}
}
accADC[ROLL] -= mcfg.accZero[ROLL];
accADC[PITCH] -= mcfg.accZero[PITCH];
accADC[YAW] -= mcfg.accZero[YAW];
}
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);
}
static void TimeoutHandler(void) {
TimerIntClear(WTIMER1_BASE, TIMER_TIMB_TIMEOUT); // Clear interrupt
writePPMAllOff(); // Turn all motors off
validRXData = false; // Indicate that connection was lost
buzzer(true); // Turn on buzzer
}
void Operation ( )
{
for ( ;; )
{
Out= 0xFF ; // Empty by default
//---------------------------------------------------------------- Empty conditions
if ( OEmpty )
{
OClearScreen() ;
Printfunction ( "Upper : Empty" ) ;
buzzer() ;
}
if ( GEmpty )
{
GClearScreen() ;
Printfunction ( "Ground : Empty" ) ;
buzzer() ;
}
//---------------------------------------------------------------- Full conditions
if ( OFull )
{
OClearScreen() ;
Printfunction ( "Upper : Full" ) ;
buzzer() ;
}
if ( GFull )
{
GClearScreen() ;
Printfunction ( "Ground : Full" ) ;
buzzer() ;
}
//---------------------------------------------------------------- One-third conditions
if( OOneThird )
{
OClearScreen( ) ;
Printfunction ( "Upper : 3/4" ) ;
delay ( 1400 ) ;
}
if( GOneThird )
{
GClearScreen( ) ;
Printfunction ( "Ground : 3/4" ) ;
delay ( 1400 ) ;
}
//---------------------------------------------------------------- Half conditions
if ( OHalf )
{
OClearScreen( ) ;
Printfunction ( "Upper : 1/2" ) ;
delay ( delaynumber ) ;
}
if ( GHalf )
{
GClearScreen( ) ;
Printfunction ( "Ground : 1/2" ) ;
delay ( delaynumber ) ;
}
//---------------------------------------------------------------- One forth conditions
if( OOneFourth )
{
if ( ( GFull ) | ( GOneThird ) | ( GHalf ) )
{
OClearScreen( ) ;
Printfunction( "Upper : 1/4" ) ;
delay ( delaynumber ) ;
OClearScreen( ) ;
Printfunction( "Upper : filling" ) ;
delay( delaynumber ) ;
LoadOn ( overhead ) ;
}
else if ( ( GOneFourth ) | ( GEmpty ) )
{
OClearScreen( ) ;
Load &= GMotor_Off ;
Printfunction( "Motor cant run" ) ;
delay ( delaynumber ) ;
OClearScreen( ) ;
Printfunction( "Lower level low" ) ;
}
}
if( GOneFourth )
{
GClearScreen( ) ;
Printfunction( "Lower : 1/4" ) ;
delay ( delaynumber ) ;
GClearScreen( ) ;
Printfunction( "Lower : filling" ) ;
delay( delaynumber ) ;
LoadOn ( ground ) ;
}
//----------------------------------------------------------------- faulty conditions
else if ( Error )
{
Commandfunction( 0x01 ) ;
Printfunction ( "Error in tanks" ) ;
buzzer() ;
}
}
}
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 int bat_cnt=0, bat_warning=0;
static char bat_warning_enabled=0;
if(++buz_cnt>=BUZZ_INTERVAL) buz_cnt=0;
//battery warning
if(++bat_cnt==100) bat_cnt=0;
if(vbat<10001) vbat=10001;
if(vbat<BATTERY_WARNING_VOLTAGE) //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-10000)/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;
}
#ifdef 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;
}
#endif
#ifdef ERROR_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;
}
#endif
#ifdef INIT_BEEP
if(!SYSTEM_initialized)
{
buz_priority|=BU_INIT;
if(buz_cnt%100<5) buz_active|=BU_INIT;
else if(buz_cnt%100<10) buz_active&=~BU_INIT;
else if(buz_cnt%100<15) buz_active|=BU_INIT;
else buz_active&=~BU_INIT;
}
else
{
buz_active&=~BU_INIT;
buz_priority&=~BU_INIT;
}
#endif
//buzzer control
unsigned char i;
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);
}
/***********************************************************************
* execute a menu action, based on line(current selected menu entry)
***********************************************************************/
static void do_menu_action(void)
{
switch(view)
{
case 0: // main menu view
switch(line)
{
case 0: // "1: Make a single beep"
buzzer(1);
break;
case 1: // "2: Make a double beep"
buzzer(2);
break;
case 2: // "3: Enter second menu view"
view = 1; // change menu view
line = 0; // on start entry
break;
case 3: // "4: Enter third menu view"
view = 2; // change menu view
line = 0; // on start entry
break;
case 4: // "5: Play trophy sound"
play_rco_sound("system_plugin", "snd_trophy");
break;
case 5: // "6: Make screenshot"
screenshot(0); // xmb only
play_rco_sound("system_plugin", "snd_system_ok");
break;
case 6: // "7: Make screenshot with menu"
screenshot(1); //
play_rco_sound("system_plugin", "snd_system_ok");
break;
case 7: // "8: Reset PS3"
stop_VSH_Menu();
delete_turnoff_flag();
sys_timer_sleep(1);
shutdown_reset(2);
break;
case 8: // "9: Shutdown PS3"
stop_VSH_Menu();
delete_turnoff_flag();
sys_timer_sleep(1);
shutdown_reset(1);
break;
}
break;
case 1: // second menu view
switch(line)
{
case 0: // 1: Back to main view"
view = line = 0;
break;
case 1: // 2: screenshot
screenshot(1);
break;
case 2: // 3: Alpha
//...
break;
case 3: // 4: Red
//...
break;
case 4: // 5: Green
//...
break;
case 5: // 6: Blue
//...
break;
case 6: // 7: test
break;
}
break;
case 2: // third menu view
switch(line)
{
case 0: // "1: Back to main view"
view = line = 0;
break;
case 1: // "2: test string..."
//...
break;
case 2: // "3: test string..."
//...
break;
case 3: // "4: test string..."
//...
break;
case 4: // "5: test string..."
//...
break;
}
break;
}
}
