You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
138 lines
3.6 KiB
138 lines
3.6 KiB
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- |
|
|
|
|
|
static void failsafe_short_on_event(enum failsafe_state fstype) |
|
{ |
|
// This is how to handle a short loss of control signal failsafe. |
|
failsafe.state = fstype; |
|
failsafe.ch3_timer_ms = millis(); |
|
gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event on, ")); |
|
switch(control_mode) |
|
{ |
|
case MANUAL: |
|
case STABILIZE: |
|
case ACRO: |
|
case FLY_BY_WIRE_A: |
|
case FLY_BY_WIRE_B: |
|
case CRUISE: |
|
case TRAINING: |
|
failsafe.saved_mode = control_mode; |
|
failsafe.saved_mode_set = 1; |
|
set_mode(CIRCLE); |
|
break; |
|
|
|
case AUTO: |
|
case GUIDED: |
|
case LOITER: |
|
if(g.short_fs_action == 1) { |
|
failsafe.saved_mode = control_mode; |
|
failsafe.saved_mode_set = 1; |
|
set_mode(CIRCLE); |
|
} |
|
break; |
|
|
|
case CIRCLE: |
|
case RTL: |
|
default: |
|
break; |
|
} |
|
gcs_send_text_fmt(PSTR("flight mode = %u"), (unsigned)control_mode); |
|
} |
|
|
|
static void failsafe_long_on_event(enum failsafe_state fstype) |
|
{ |
|
// This is how to handle a long loss of control signal failsafe. |
|
gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Long event on, ")); |
|
// If the GCS is locked up we allow control to revert to RC |
|
hal.rcin->clear_overrides(); |
|
failsafe.state = fstype; |
|
switch(control_mode) |
|
{ |
|
case MANUAL: |
|
case STABILIZE: |
|
case ACRO: |
|
case FLY_BY_WIRE_A: |
|
case FLY_BY_WIRE_B: |
|
case CRUISE: |
|
case TRAINING: |
|
case CIRCLE: |
|
set_mode(RTL); |
|
break; |
|
|
|
case AUTO: |
|
case GUIDED: |
|
case LOITER: |
|
if(g.long_fs_action == 1) { |
|
set_mode(RTL); |
|
} |
|
break; |
|
|
|
case RTL: |
|
default: |
|
break; |
|
} |
|
gcs_send_text_fmt(PSTR("flight mode = %u"), (unsigned)control_mode); |
|
} |
|
|
|
static void failsafe_short_off_event() |
|
{ |
|
// We're back in radio contact |
|
gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event off")); |
|
failsafe.state = FAILSAFE_NONE; |
|
|
|
// re-read the switch so we can return to our preferred mode |
|
// -------------------------------------------------------- |
|
if (control_mode == CIRCLE && failsafe.saved_mode_set) { |
|
failsafe.saved_mode_set = 0; |
|
set_mode(failsafe.saved_mode); |
|
} |
|
} |
|
|
|
void low_battery_event(void) |
|
{ |
|
if (battery.low_batttery) { |
|
return; |
|
} |
|
gcs_send_text_fmt(PSTR("Low Battery %.2fV Used %.0f mAh"), |
|
battery.voltage, battery.current_total_mah); |
|
set_mode(RTL); |
|
aparm.throttle_cruise.load(); |
|
battery.low_batttery = true; |
|
} |
|
|
|
//////////////////////////////////////////////////////////////////////////////// |
|
// repeating event control |
|
|
|
/* |
|
update state for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY |
|
*/ |
|
static void update_events(void) |
|
{ |
|
if (event_state.repeat == 0 || (millis() - event_state.start_time_ms) < event_state.delay_ms) { |
|
return; |
|
} |
|
|
|
// event_repeat = -1 means repeat forever |
|
if (event_state.repeat != 0) { |
|
event_state.start_time_ms = millis(); |
|
|
|
switch (event_state.type) { |
|
case EVENT_TYPE_SERVO: |
|
hal.rcout->enable_ch(event_state.rc_channel); |
|
if (event_state.repeat & 1) { |
|
servo_write(event_state.rc_channel, event_state.undo_value); |
|
} else { |
|
servo_write(event_state.rc_channel, event_state.servo_value); |
|
} |
|
break; |
|
|
|
case EVENT_TYPE_RELAY: |
|
relay.toggle(); |
|
break; |
|
} |
|
|
|
if (event_state.repeat > 0) { |
|
event_state.repeat--; |
|
} |
|
} |
|
}
|
|
|