inline std::error_code SimulatedFailure::trigger(FailureType failure_type, std::error_code& ec) noexcept
{
if (check_trigger(failure_type)) {
ec = make_error_code(failure_type);
}
else {
ec = std::error_code();
}
return ec;
}
static void sample(void *arg, long period)
{
int n;
ctrl_shm->watchdog = 0;
if (ctrl_shm->state == RESET) {
/* sampling interrupted, reset everything */
ctrl_shm->curr = 0;
ctrl_shm->start = ctrl_shm->curr;
ctrl_shm->samples = 0;
ctrl_shm->force_trig = 0;
/* reset completed, set new state */
ctrl_shm->state = IDLE;
}
ctrl_rt->mult_cntr++;
if (ctrl_rt->mult_cntr < ctrl_shm->mult) {
/* not time to do anything yet */
return;
}
/* reset counter */
ctrl_rt->mult_cntr = 0;
/* run the sampling state machine */
switch (ctrl_shm->state) {
case IDLE:
/* do nothing while waiting for INIT */
break;
case INIT:
/* init start pointer, curr pointer, sample count */
ctrl_shm->curr = 0;
ctrl_shm->start = ctrl_shm->curr;
ctrl_shm->samples = 0;
ctrl_shm->force_trig = 0;
ctrl_rt->auto_timer = 0;
/* get info about channels */
for (n = 0; n < 16; n++) {
ctrl_rt->data_addr[n] = SHMPTR(ctrl_shm->data_offset[n]);
ctrl_rt->data_type[n] = ctrl_shm->data_type[n];
ctrl_rt->data_len[n] = ctrl_shm->data_len[n];
}
/* set next state */
ctrl_shm->state = PRE_TRIG;
break;
case PRE_TRIG:
/* acquire a sample */
capture_sample();
/* increment sample counter */
ctrl_shm->samples++;
/* check if all pre-trigger samples captured */
if (ctrl_shm->samples >= ctrl_shm->pre_trig) {
/* yes - start waiting for trigger */
ctrl_shm->state = TRIG_WAIT;
/* dummy call to preset 'compare_result' */
check_trigger();
}
break;
case TRIG_WAIT:
/* acquire a sample */
capture_sample();
/* increment sample counter */
ctrl_shm->samples++;
/* check if trigger condition met */
if (check_trigger()) {
/* yes - start acquiring post trigger data */
ctrl_shm->state = POST_TRIG;
} else {
/* no, discard oldest pre-trig sample */
ctrl_shm->samples--;
ctrl_shm->start += ctrl_shm->sample_len;
/* is there a valid sample here, or end of buffer? */
if ((ctrl_shm->start + ctrl_shm->sample_len) > ctrl_shm->buf_len) {
/* end of buffer, wrap back to beginning */
ctrl_shm->start = 0;
}
}
break;
case POST_TRIG:
/* acquire a sample */
capture_sample();
/* increment sample counter */
ctrl_shm->samples++;
/* check if all post-trigger samples captured */
if (ctrl_shm->samples >= ctrl_shm->rec_len) {
/* yes - stop sampling and cleanup */
ctrl_shm->state = DONE;
}
break;
case DONE:
/* do nothing while GUI displays waveform */
break;
default:
/* shouldn't get here - if we do, set a legal state */
ctrl_shm->state = IDLE;
break;
}
/* done */
}
gboolean trigger_init(void)
{
const gchar *udev_path = NULL;
struct udev_device *dev;
int ret = 0;
/* Create the udev object */
udev = udev_new();
if (!udev)
{
log_err("Can't create udev\n");
return 1;
}
udev_path = check_trigger();
if(udev_path)
dev = udev_device_new_from_syspath(udev, udev_path);
else
{
log_err("No trigger path. Not starting trigger.\n");
return 1;
}
if (!dev)
{
log_err("Unable to find the trigger device.");
return 1;
}
else
{
dev_name = udev_device_get_sysname(dev);
log_debug("device name = %s\n", dev_name);
}
mon = udev_monitor_new_from_netlink (udev, "udev");
if (!mon)
{
log_err("Unable to monitor the netlink\n");
/* communicate failure, mainloop will exit and call appropriate clean-up */
return 1;
}
ret = udev_monitor_filter_add_match_subsystem_devtype(mon, get_trigger_subsystem(), NULL);
if(ret != 0)
{
log_err("Udev match failed.\n");
return 1;
}
ret = udev_monitor_enable_receiving (mon);
if(ret != 0)
{
log_err("Failed to enable monitor recieving.\n");
return 1;
}
/* check if we are already connected */
udev_parse(dev);
iochannel = g_io_channel_unix_new(udev_monitor_get_fd(mon));
watch_id = g_io_add_watch_full(iochannel, 0, G_IO_IN, monitor_udev, NULL, notify_issue);
/* everything went well */
log_debug("Trigger enabled!\n");
return 0;
}
/**
* Main apply handler.
*
* Checks for unpaid items before applying.
* @param op ::object causing tmp to be applied.
* @param tmp ::object being applied.
* @param aflag Special (always apply/unapply) flags. Nothing is done
* with them in this function - they are passed to apply_special().
* @retval 0 Player or monster can't apply objects of that type.
* @retval 1 Has been applied, or there was an error applying the object.
* @retval 2 Objects of that type can't be applied if not in
* inventory. */
int manual_apply(object *op, object *tmp, int aflag)
{
if (tmp->head)
{
tmp = tmp->head;
}
if (op->type == PLAYER)
{
CONTR(op)->praying = 0;
}
if (QUERY_FLAG(tmp, FLAG_UNPAID) && !QUERY_FLAG(tmp, FLAG_APPLIED))
{
if (op->type == PLAYER)
{
new_draw_info(NDI_UNIQUE, op, "You should pay for it first.");
return 1;
}
/* Monsters just skip unpaid items */
else
{
return 0;
}
}
/* Monsters must not apply random chests, nor magic_mouths with a counter */
if (op->type != PLAYER && tmp->type == TREASURE)
{
return 0;
}
/* Trigger the APPLY event */
if (!(aflag & AP_NO_EVENT) && trigger_event(EVENT_APPLY, op, tmp, NULL, NULL, aflag, 0, 0, SCRIPT_FIX_ACTIVATOR))
{
return 1;
}
aflag &= ~AP_NO_EVENT;
/* Control apply by controling a set exp object level or player exp level */
if (tmp->item_level)
{
int tmp_lev;
if (tmp->item_skill)
{
tmp_lev = find_skill_exp_level(op, tmp->item_skill);
}
else
{
tmp_lev = op->level;
}
if (tmp->item_level > tmp_lev)
{
new_draw_info(NDI_UNIQUE, op, "The item level is too high to apply.");
return 1;
}
}
switch (tmp->type)
{
case HOLY_ALTAR:
new_draw_info_format(NDI_UNIQUE, op, "You touch the %s.", tmp->name);
if (change_skill(op, SK_PRAYING))
{
pray_at_altar(op, tmp);
}
else
{
new_draw_info(NDI_UNIQUE, op, "Nothing happens. It seems you miss the right skill.");
}
return 1;
break;
case HANDLE:
new_draw_info(NDI_UNIQUE, op, "You turn the handle.");
play_sound_map(op->map, op->x, op->y, SOUND_TURN_HANDLE, SOUND_NORMAL);
tmp->value = tmp->value ? 0 : 1;
SET_ANIMATION(tmp, ((NUM_ANIMATIONS(tmp) / NUM_FACINGS(tmp)) * tmp->direction) + tmp->value);
update_object(tmp, UP_OBJ_FACE);
push_button(tmp);
return 1;
case TRIGGER:
if (check_trigger(tmp, op))
{
new_draw_info(NDI_UNIQUE, op, "You turn the handle.");
play_sound_map(tmp->map, tmp->x, tmp->y, SOUND_TURN_HANDLE, SOUND_NORMAL);
}
else
{
new_draw_info(NDI_UNIQUE, op, "The handle doesn't move.");
}
return 1;
case EXIT:
if (op->type != PLAYER || !tmp->map)
{
return 0;
}
/* If no map path specified, we assume it is the map path of the exit. */
if (!EXIT_PATH(tmp))
{
FREE_AND_ADD_REF_HASH(EXIT_PATH(tmp), tmp->map->path);
}
if (!EXIT_PATH(tmp) || !is_legal_2ways_exit(op, tmp) || (EXIT_Y(tmp) == -1 && EXIT_X(tmp) == -1))
{
new_draw_info_format(NDI_UNIQUE, op, "The %s is closed.", query_name(tmp, NULL));
}
else
{
/* Don't display messages for random maps. */
if (tmp->msg && strncmp(EXIT_PATH(tmp), "/!", 2) && strncmp(EXIT_PATH(tmp), "/random/", 8))
{
new_draw_info(NDI_NAVY, op, tmp->msg);
}
enter_exit(op, tmp);
}
return 1;
case SIGN:
apply_sign(op, tmp);
return 1;
case BOOK:
if (op->type == PLAYER)
{
apply_book(op, tmp);
return 1;
}
return 0;
case SKILLSCROLL:
if (op->type == PLAYER)
{
apply_skillscroll(op, tmp);
return 1;
}
return 0;
case SPELLBOOK:
if (op->type == PLAYER)
{
apply_spellbook(op, tmp);
return 1;
}
return 0;
case SCROLL:
apply_scroll(op, tmp);
return 1;
case POTION:
(void) apply_potion(op, tmp);
return 1;
case LIGHT_APPLY:
apply_player_light(op, tmp);
return 1;
case LIGHT_REFILL:
apply_player_light_refill(op, tmp);
return 1;
/* Eneq(@csd.uu.se): Handle apply on containers. */
case CLOSE_CON:
if (op->type == PLAYER)
{
(void) esrv_apply_container(op, tmp->env);
}
return 1;
case CONTAINER:
if (op->type == PLAYER)
{
(void) esrv_apply_container(op, tmp);
}
return 1;
case TREASURE:
apply_treasure(op, tmp);
return 1;
case WEAPON:
case ARMOUR:
case BOOTS:
case GLOVES:
case AMULET:
case GIRDLE:
case BRACERS:
case SHIELD:
case HELMET:
case RING:
case CLOAK:
case WAND:
case ROD:
case HORN:
case SKILL:
case BOW:
case SKILL_ITEM:
/* Not in inventory */
if (tmp->env != op)
{
return 2;
}
(void) apply_special(op, tmp, aflag);
return 1;
case DRINK:
case FOOD:
case FLESH:
apply_food(op, tmp);
return 1;
case POISON:
apply_poison(op, tmp);
return 1;
case SAVEBED:
if (op->type == PLAYER)
{
apply_savebed(op);
return 1;
}
return 0;
case ARMOUR_IMPROVER:
if (op->type == PLAYER)
{
apply_armour_improver(op, tmp);
return 1;
}
return 0;
case WEAPON_IMPROVER:
apply_weapon_improver(op, tmp);
return 1;
case CLOCK:
if (op->type == PLAYER)
{
timeofday_t tod;
get_tod(&tod);
new_draw_info_format(NDI_UNIQUE, op, "It is %d minute%s past %d o'clock %s", tod.minute + 1, ((tod.minute + 1 < 2) ? "" : "s"), ((tod.hour % (HOURS_PER_DAY / 2) == 0) ? (HOURS_PER_DAY / 2) : ((tod.hour) % (HOURS_PER_DAY / 2))), ((tod.hour >= (HOURS_PER_DAY / 2)) ? "pm" : "am"));
return 1;
}
return 0;
case POWER_CRYSTAL:
apply_power_crystal(op, tmp);
return 1;
/* For lighting torches/lanterns/etc */
case LIGHTER:
if (op->type == PLAYER)
{
apply_lighter(op, tmp);
return 1;
}
return 0;
/* So the below default case doesn't execute for these objects,
* even if they have message. */
case DOOR:
return 0;
/* Nothing from the above... but show a message if it has one. */
default:
if (tmp->msg)
{
new_draw_info(NDI_UNIQUE, op, tmp->msg);
return 1;
}
return 0;
}
}
/**
* 'victim' moves onto 'trap' (trap has FLAG_WALK_ON or FLAG_FLY_ON set) or
* 'victim' leaves 'trap' (trap has FLAG_WALK_OFF or FLAG_FLY_OFF) set.
*
* I added the flags parameter to give the single events more information
* about whats going on:
*
* Most important is the "MOVE_APPLY_VANISHED" flag.
* If set, a object has left a tile but "vanished" and not moved (perhaps
* it exploded or something). This means that some events are not
* triggered like trapdoors or teleporter traps for example which have a
* "FLY/MOVE_OFF" set. This will avoid that they touch invalid objects.
* @param trap Object victim moved on.
* @param victim The object that moved on trap.
* @param originator Player, monster or other object that caused 'victim'
* to move onto 'trap'. Will receive messages caused by this action. May
* be NULL, however, some types of traps require an originator to
* function.
* @param flags Flags. */
void move_apply(object *trap, object *victim, object *originator, int flags)
{
static int recursion_depth = 0;
/* move_apply() is the most likely candidate for causing unwanted and
* possibly unlimited recursion. */
/* The following was changed because it was causing perfeclty correct
* maps to fail. 1) it's not an error to recurse:
* rune detonates, summoning monster. monster lands on nearby rune.
* nearby rune detonates. This sort of recursion is expected and
* proper. This code was causing needless crashes. */
if (recursion_depth >= 500)
{
LOG(llevDebug, "WARNING: move_apply(): aborting recursion [trap arch %s, name %s; victim arch %s, name %s]\n", trap->arch->name, trap->name, victim->arch->name, victim->name);
return;
}
if (trap->head)
{
trap = trap->head;
}
/* Trigger the TRIGGER event */
if (trigger_event(EVENT_TRIGGER, victim, trap, originator, NULL, 0, 0, 0, SCRIPT_FIX_NOTHING))
{
return;
}
recursion_depth++;
switch (trap->type)
{
/* these objects can trigger other objects connected to them.
* We need to check them at map loading time and other special
* events to be sure to have a 100% working map state. */
case BUTTON:
case PEDESTAL:
update_button(trap);
break;
case TRIGGER_BUTTON:
case TRIGGER_PEDESTAL:
case TRIGGER_ALTAR:
check_trigger(trap, victim);
break;
case CHECK_INV:
check_inv(victim, trap);
break;
/* these objects trigger to but they are "instant".
* We don't need to check them when loading. */
case ALTAR:
/* sacrifice victim on trap */
apply_altar(trap, victim, originator);
break;
case CONVERTER:
if (!(flags & MOVE_APPLY_VANISHED))
{
convert_item(victim, trap);
}
break;
case PLAYERMOVER:
break;
/* should be walk_on/fly_on only */
case SPINNER:
if (victim->direction)
{
if ((victim->direction = victim->direction + trap->direction) > 8)
{
victim->direction = (victim->direction % 8) + 1;
}
update_turn_face(victim);
}
break;
case DIRECTOR:
if (victim->direction)
{
victim->direction = trap->direction;
update_turn_face(victim);
}
break;
/* no need to hit anything */
case MMISSILE:
if (IS_LIVE(victim) && !(flags&MOVE_APPLY_VANISHED))
{
tag_t trap_tag = trap->count;
hit_player(victim, trap->stats.dam, trap, AT_MAGIC);
if (!was_destroyed(trap, trap_tag))
{
remove_ob(trap);
}
check_walk_off(trap, NULL, MOVE_APPLY_VANISHED);
}
break;
case THROWN_OBJ:
if (trap->inv == NULL || (flags & MOVE_APPLY_VANISHED))
{
break;
}
/* fallthrough */
case ARROW:
/* bad bug: monster throw a object, make a step forwards, step on object ,
* trigger this here and get hit by own missile - and will be own enemy.
* Victim then is his own enemy and will start to kill herself (this is
* removed) but we have not synced victim and his missile. To avoid senseless
* action, we avoid hits here */
if ((IS_LIVE(victim) && trap->speed) && trap->owner != victim)
{
hit_with_arrow(trap, victim);
}
break;
case CONE:
case LIGHTNING:
break;
case BULLET:
if ((QUERY_FLAG(victim, FLAG_NO_PASS) || IS_LIVE(victim)) && !(flags & MOVE_APPLY_VANISHED))
{
check_fired_arch(trap);
}
break;
case TRAPDOOR:
{
int max, sound_was_played;
object *ab;
if ((flags & MOVE_APPLY_VANISHED))
{
break;
}
if (!trap->value)
{
sint32 tot;
for (ab = trap->above, tot = 0; ab != NULL; ab = ab->above)
{
if (!QUERY_FLAG(ab, FLAG_FLYING))
{
tot += (ab->nrof ? ab->nrof : 1) * ab->weight + ab->carrying;
}
}
if (!(trap->value = (tot > trap->weight) ? 1 : 0))
{
break;
}
SET_ANIMATION(trap, (NUM_ANIMATIONS(trap) / NUM_FACINGS(trap)) * trap->direction + trap->value);
update_object(trap, UP_OBJ_FACE);
}
for (ab = trap->above, max = 100, sound_was_played = 0; --max && ab && !QUERY_FLAG(ab, FLAG_FLYING); ab = ab->above)
{
if (!sound_was_played)
{
play_sound_map(trap->map, trap->x, trap->y, SOUND_FALL_HOLE, SOUND_NORMAL);
sound_was_played = 1;
}
if (ab->type == PLAYER)
{
new_draw_info(NDI_UNIQUE, ab, "You fall into a trapdoor!");
}
transfer_ob(ab, EXIT_X(trap), EXIT_Y(trap), trap->last_sp, ab, trap);
}
break;
}
case PIT:
/* Pit not open? */
if ((flags & MOVE_APPLY_VANISHED) || trap->stats.wc > 0)
{
break;
}
play_sound_map(victim->map, victim->x, victim->y, SOUND_FALL_HOLE, SOUND_NORMAL);
if (victim->type == PLAYER)
{
new_draw_info(NDI_UNIQUE, victim, "You fall through the hole!\n");
}
transfer_ob(victim->head ? victim->head : victim, EXIT_X(trap), EXIT_Y(trap), trap->last_sp, victim, trap);
break;
case EXIT:
/* If no map path specified, we assume it is the map path of the exit. */
if (!EXIT_PATH(trap))
{
FREE_AND_ADD_REF_HASH(EXIT_PATH(trap), trap->map->path);
}
if (!(flags & MOVE_APPLY_VANISHED) && victim->type == PLAYER && EXIT_PATH(trap) && EXIT_Y(trap) != -1 && EXIT_X(trap) != -1)
{
/* Basically, don't show exits leading to random maps the players output. */
if (trap->msg && strncmp(EXIT_PATH(trap), "/!", 2) && strncmp(EXIT_PATH(trap), "/random/", 8))
{
new_draw_info(NDI_NAVY, victim, trap->msg);
}
enter_exit(victim, trap);
}
break;
case SHOP_MAT:
if (!(flags & MOVE_APPLY_VANISHED))
{
apply_shop_mat(trap, victim);
}
break;
/* Drop a certain amount of gold, and have one item identified */
case IDENTIFY_ALTAR:
if (!(flags & MOVE_APPLY_VANISHED))
{
apply_identify_altar(victim, trap, originator);
}
break;
case SIGN:
/* Only player should be able read signs */
if (victim->type == PLAYER)
{
apply_sign(victim, trap);
}
break;
case CONTAINER:
if (victim->type == PLAYER)
{
(void) esrv_apply_container(victim, trap);
}
break;
case RUNE:
if (!(flags & MOVE_APPLY_VANISHED) && trap->level && IS_LIVE(victim))
{
spring_trap(trap, victim);
}
break;
#if 0
/* we don't have this atm. */
case DEEP_SWAMP:
if (!(flags & MOVE_APPLY_VANISHED))
{
walk_on_deep_swamp(trap, victim);
}
break;
#endif
default:
LOG(llevDebug, "name %s, arch %s, type %d with fly/walk on/off not handled in move_apply()\n", trap->name, trap->arch->name, trap->type);
break;
}
recursion_depth--;
}
/**
* Drive instruction handler function.
*
* Very long but versitile function used to drive around the world
* (maybe not in 80 days).
* @param[in,out] order The order which specifies what exactly
* should be done.
* @todo The function is partially really ugly. Refactor it.
*/
void drive_instruction(order_t *order) {
// Extract the type of triggers to be used
uint8_t trigger_type_left = order->data[0] & 0x30;
uint8_t trigger_type_right = order->data[0] & 0xc0;
// Import the option for ABS
extern uint8_t ACTIVE_BRAKE_WHEN_TRIGGER_REACHED;
if (DEBUG_ENABLE) {
debug_write_integer(PSTR("order_functions.c : drive_instruction() : trigger_type_left = "), trigger_type_left);
debug_write_integer(PSTR("order_functions.c : drive_instruction() : trigger_type_right = "), trigger_type_right);
}
// This if shouldn't be taken, just to make sure
if (order->status & ORDER_STATUS_DONE) {
if (DEBUG_ENABLE) {
debug_write_string_p(PSTR("order_functions.c : drive_instruction() : status = ORDER_STATUS_DONE\n"));
}
return;
}
// Instruciton is already running
if (order->status & ORDER_STATUS_STARTED) {
// Local variable to save the checkTrigger return value for debug output
uint16_t result = 0;
if (DEBUG_ENABLE) {
debug_write_string_p(PSTR("order_functions.c : drive_instruction() : status = ORDER_STATUS_STARTED\n"));
}
// Check Left trigger
result = check_trigger(trigger_type_left, WHEEL_LEFT);
if (DEBUG_ENABLE) {
debug_write_integer(PSTR("order_functions.c : drive_instruction() : checkTrigger(LEFT) = "), result);
}
// Left Trigger reached in this Iteration?
if (!(order->status & ORDER_STATUS_TRIGGER_LEFT_REACHED) &&
result == 0) {
// Set the Trigger reached flag
order->status |= ORDER_STATUS_TRIGGER_LEFT_REACHED;
// set the ABS ref position
drive_brake_active_set(WHEEL_LEFT);
if (DEBUG_ENABLE) {
debug_write_string_p(PSTR("order_functions.c : drive_instruction() : status = ORDER_STATUS_TRIGGER_LEFT_REACHED\n"));
}
}
// Right trigger reached
result = check_trigger(trigger_type_right, WHEEL_RIGHT);
if (DEBUG_ENABLE) {
debug_write_integer(PSTR("order_functions.c : drive_instruction() : checkTrigger(RIGHT) = "), result);
}
// Right Trigger reached in this Iteration?
if (!(order->status & ORDER_STATUS_TRIGGER_RIGHT_REACHED) &&
result == 0 ) {
// Set the Trigger reached flag
order->status |= ORDER_STATUS_TRIGGER_RIGHT_REACHED;
// set the ABS ref position
drive_brake_active_set(WHEEL_RIGHT);
if (DEBUG_ENABLE) {
debug_write_string_p(PSTR("order_functions.c : drive_instruction() : status = ORDER_STATUS_TRIGGER_RIGHT_REACHED\n"));
}
}
// If in PID with D mode one reached trigger equals all trigger reached
if ((order->status & ORDER_STATUS_TRIGGER_RIGHT_REACHED ||
order->status & ORDER_STATUS_TRIGGER_LEFT_REACHED) &&
trigger_type_left == 0x30) {
order->status |= ORDER_STATUS_TRIGGER_LEFT_REACHED + ORDER_STATUS_TRIGGER_RIGHT_REACHED;
}
// both triggers reached, ergo order is done
if (order->status & ORDER_STATUS_TRIGGER_RIGHT_REACHED &&
order->status & ORDER_STATUS_TRIGGER_LEFT_REACHED) {
if (DEBUG_ENABLE) {
debug_write_string_p(PSTR("order_functions.c : drive_instruction() : All trigger reached\n"));
}
order->status |= ORDER_STATUS_DONE;
// stop the wheels
drive_use_pid(WHEEL_BOTH, 0);
wheel_clear_difference();
}
// When it is wanted, we will use active breaking on the wheel where the trigger has been reached
if (ACTIVE_BRAKE_WHEN_TRIGGER_REACHED) {
if (order->status & ORDER_STATUS_TRIGGER_LEFT_REACHED) {
drive_brake_active(WHEEL_LEFT);
} else if (order->status & ORDER_STATUS_TRIGGER_RIGHT_REACHED) {
drive_brake_active(WHEEL_RIGHT);
}
}
// Use error-correction while driving every 100 ms
if (local_time_flags & TIMER_100MS) {
if (trigger_type_left == 0x30) { // Use function for both wheels to use PID mode with D
drive_use_pid(WHEEL_BOTH, order->data[1]);
} else {
// Only drive the wheels if the triggers haven't been reached
if (!(order->status & ORDER_STATUS_TRIGGER_LEFT_REACHED)) {
drive_use_pid(WHEEL_LEFT, order->data[1]);
}
if (!(order->status & ORDER_STATUS_TRIGGER_RIGHT_REACHED)) {
drive_use_pid(WHEEL_RIGHT, order->data[2]);
}
}
}
} else { //Instruction isn't running
// Build the values for the specified triggers, even if there aren't any triggers present
int16_t trigger_value_left = 0;
int16_t trigger_value_right = 0;
if (DEBUG_ENABLE) {
debug_write_string_p(PSTR("order_functions.c : drive_instruction() : Start execution\n"));
debug_write_integer(PSTR("order_functions.c : drive_instruction() : speed_left = "), order->data[1]);
debug_write_integer(PSTR("order_functions.c : drive_instruction() : speed_right = "), order->data[2]);
}
// -128 is not a accepted speed, so we correct it down by 1
if (order->data[1] == 128) {
order->data[1] = -127;
}
if (order->data[2] == 128) {
order->data[2] = -127;
}
trigger_value_left = ((order->data[3] << 8) + order->data[4]);
trigger_value_right = ((order->data[5] << 8) + order->data[6]);
if (trigger_type_right == 0xc0) { // Set new differential correction
trigger_value_left = (order->data[1] << 8) + order->data[2];
wheel_write_difference(trigger_value_left);
// We are done with this order
order->status |= ORDER_STATUS_DONE;
return;
}
if (trigger_type_left == 0x30) { // PID with differential correction
trigger_value_left = trigger_value_right = ((order->data[2] << 8) + order->data[3]);
}
// Set the triggers for each wheel
if (DEBUG_ENABLE) {
debug_write_integer(PSTR("order_functions.c : drive_instruction() : trigger_value_left = "), trigger_value_left);
debug_write_integer(PSTR("order_functions.c : drive_instruction() : trigger_value_right = "), trigger_value_right);
}
// Set the Trigger values
set_trigger(trigger_type_left, WHEEL_LEFT, trigger_value_left);
set_trigger(trigger_type_right, WHEEL_RIGHT, trigger_value_right);
// Set Status to started as setup is complete
order->status |= ORDER_STATUS_STARTED;
}
}
inline void SimulatedFailure::trigger(FailureType failure_type)
{
if (check_trigger(failure_type))
throw SimulatedFailure(make_error_code(failure_type));
}
