void main(void)
{
int i;
unsigned char xdata *seg = (unsigned char xdata *)(0x500);
//unsigned char xdata *jump_fp;
//void(code *fp)();
blink_test();
ECO_PAGE_TABLE[0] = 2;
PAGE_ADDR_L = 10;
PAGE_ADDR_H = 11;
//init LED
P0_DIR &= ~0x28;
P0_ALT &= ~0x28;
store_cpu_rate(16);
for(i=0;i<6;i++)
{
//LED blink
blink_led();
mdelay(200);
}
mdelay(800);
eeprom_init();
//Get data from 0x1000 address
for(i=0;i<64;i++)
{
*(seg+i) = eeprom_read(ECO_ADDR_SHIFT(4096) +i);
}
mdelay(400);
//function pointer to blink_fast()
//jump_fp = (unsigned char xdata *)((1280));
//((void (code *)(void))jump_fp)();
/*JUMP_ADDR = 0x500;
#pragma asm
MOV A,JUMP_ADDR
MOV DPH,A
MOV A,JUMP_ADDR+01H
MOV DPL,A
CLR A
JMP @A+DPTR
#pragma endasm
*/
blink_fast();
/*
#pragma asm
//LCALL 0x500
CLR A
;ADDR_LOW
MOV A,#0CH
PUSH ACC
CLR A
;ADDR_HIGH
MOV A,#0AH
PUSH ACC
#pragma endasm
*/
//ECO_PAGE_MANAGER();
mdelay(800);
for(i=0;i<10;i++)
{
blink_led();
mdelay(200);
}
mdelay(400);
}
void run_led_driver() {
prep_leds();
switch(led_state) {
case led_off:
break;
case led_start:
turn_on(REDLED);
break;
case led_setup_start:
turn_on(REDLED);
blink_slow(YELLOWLED);
blink_slow(GREENLED1);
blink_slow(GREENLED2);
break;
case led_setup_slave_assoc:
turn_on(REDLED);
blink_slow(YELLOWLED);
turn_on(GREENLED1);
blink_slow(GREENLED2);
break;
case led_setup_slave_found_master:
turn_on(REDLED);
blink_slow(YELLOWLED);
turn_on(GREENLED1);
turn_on(GREENLED2);
break;
case led_setup_master_assoc:
turn_on(REDLED);
blink_fast(YELLOWLED);
turn_on(GREENLED1);
turn_on(GREENLED2);
break;
case led_main_start:
turn_on(REDLED);
blink_slow(YELLOWLED);
blink_slow_sliding(GREENLED1);
blink_slow_alternating(GREENLED2);
break;
case led_main_assoc:
turn_on(REDLED);
turn_on(YELLOWLED);
blink_slow_alternating(GREENLED1);
break;
case led_main_connected:
turn_on(REDLED);
turn_on(YELLOWLED);
turn_on(GREENLED1);
blink_slow(GREENLED2);
break;
case led_error:
blink_slow(REDLED + YELLOWLED + GREENLED1);
break;
}
update_leds();
}
void Craft::loop_vtol() {
switch (m_current) {
case STOPPED:
m_attitude.calibrate();
m_current = STOPPED_READY;
case STOPPED_READY:
blink_slow();
if (m_userDemand.throttle >= 2000) {
m_current = STOPPED_STARTUP;
}
break;
case STOPPED_STARTUP:
if (m_userDemand.throttle <= 48) {
m_current = NORMAL;
}
break;
case LAUNCH:
break;
case NORMAL:
blink_fast();
m_currentControl = m_controlSchemes[0];
m_currentControl->update();
// TODO check for switch to/from state
break;
case HOLD:
blink_fast();
m_currentControl = m_controlSchemes[1];
m_currentControl->update();
// TODO check for switch from/to state
break;
case ACRO:
blink_fast();
m_currentControl = m_controlSchemes[2];
m_currentControl->update();
// TODO
break;
case WAYPOINT:
blink_fast();
m_currentControl = m_controlSchemes[3];
m_currentControl->update();
// TODO
break;
case RETURN_HOME:
blink_fast();
// TODO set coords
m_currentControl = m_controlSchemes[3];
m_currentControl->update();
// TODO
break;
case LOW_BATTERY:
// TODO pulse motors, normal control
break;
case STOP:
// TODO
break;
case REPLAY:
// TODO implement these modes
break;
case FAULT:
default:
// TODO power off
m_motors.set(0, 0, 0, 0);
blink_fault();
break;
}
}
