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