char *levenshtein(char *str, UNUSED char **env)
{
char **dic;
char **command;
int i;
int pos;
i = 0;
pos = 0;
command = cut_command(str);
if ((dic = gen_data(save_my_env(NULL))) == NULL)
return (str);
while (command && command[i] != NULL)
{
if (i == 0 || is_separator_ext(command[i - 1][0]) == 0)
{
if (check_command(command[i], save_my_env(NULL)) == -1)
{
pos = where_is_space(command[i]);
command[i] = change_command(command[i],
leven(dic, po_s(command[i], pos)));
}
}
i++;
}
free_array_char(dic);
return (tab_string(command));
}
// check for Failsafe conditions. This is called at 10Hz by the main
// ArduPlane code
void
APM_OBC::check(APM_OBC::control_mode mode,
uint32_t last_heartbeat_ms,
uint32_t last_gps_fix_ms)
{
// we always check for fence breach
if (geofence_breached()) {
if (!_terminate) {
gcs_send_text_fmt(PSTR("Fence TERMINATE"));
_terminate.set(1);
}
}
// tell the failsafe board if we are in manual control
// mode. This tells it to pass through controls from the
// receiver
if (_manual_pin != -1) {
if (_manual_pin != _last_manual_pin) {
pinMode(_manual_pin, OUTPUT);
_last_manual_pin = _manual_pin;
}
digitalWrite(_manual_pin, mode==OBC_MANUAL);
}
uint32_t now = millis();
bool gcs_link_ok = ((now - last_heartbeat_ms) < 10000);
bool gps_lock_ok = ((now - last_gps_fix_ms) < 3000);
switch (_state) {
case STATE_PREFLIGHT:
// we startup in preflight mode. This mode ends when
// we first enter auto.
if (mode == OBC_AUTO) {
gcs_send_text_fmt(PSTR("Starting OBC_AUTO"));
_state = STATE_AUTO;
}
break;
case STATE_AUTO:
// this is the normal mode.
if (!gcs_link_ok) {
gcs_send_text_fmt(PSTR("State DATA_LINK_LOSS"));
_state = STATE_DATA_LINK_LOSS;
if (_wp_comms_hold) {
if (_command_index != NULL) {
_saved_wp = _command_index->get();
}
change_command(_wp_comms_hold);
}
break;
}
if (!gps_lock_ok) {
gcs_send_text_fmt(PSTR("State GPS_LOSS"));
_state = STATE_GPS_LOSS;
if (_wp_gps_loss) {
if (_command_index != NULL) {
_saved_wp = _command_index->get();
}
change_command(_wp_gps_loss);
}
break;
}
break;
case STATE_DATA_LINK_LOSS:
if (!gps_lock_ok) {
// losing GPS lock when data link is lost
// leads to termination
gcs_send_text_fmt(PSTR("Dual loss TERMINATE"));
_terminate.set(1);
} else if (gcs_link_ok) {
_state = STATE_AUTO;
gcs_send_text_fmt(PSTR("GCS OK"));
if (_saved_wp != 0) {
change_command(_saved_wp);
_saved_wp = 0;
}
}
break;
case STATE_GPS_LOSS:
if (!gcs_link_ok) {
// losing GCS link when GPS lock lost
// leads to termination
gcs_send_text_fmt(PSTR("Dual loss TERMINATE"));
_terminate.set(1);
} else if (gps_lock_ok) {
gcs_send_text_fmt(PSTR("GPS OK"));
_state = STATE_AUTO;
if (_saved_wp != 0) {
change_command(_saved_wp);
_saved_wp = 0;
}
}
break;
}
// if we are not terminating or if there is a separate terminate
// pin configured then toggle the heartbeat pin at 10Hz
if (_heartbeat_pin != -1 && (_terminate_pin != -1 || !_terminate)) {
if (_heartbeat_pin != _last_heartbeat_pin) {
pinMode(_heartbeat_pin, OUTPUT);
_last_heartbeat_pin = _heartbeat_pin;
}
_heartbeat_pin_value = !_heartbeat_pin_value;
digitalWrite(_heartbeat_pin, _heartbeat_pin_value);
}
// set the terminate pin
if (_terminate_pin != -1) {
if (_terminate_pin != _last_terminate_pin) {
pinMode(_terminate_pin, OUTPUT);
_last_terminate_pin = _terminate_pin;
}
digitalWrite(_terminate_pin, _terminate?HIGH:LOW);
}
}
