int handle_event_going_down(int event, int order_table[N_FLOORS][N_BUTTONS]) {
int next_state = GOING_DOWN;
switch (event) {
case EMERGENCY_BUTTON:
change_speed(S_DOWN, S_STOP);
activate_emergency(order_table);
next_state = EMERGENCY;
break;
case GO_UP:
change_speed(S_DOWN, S_UP);
next_state = GOING_UP;
break;
case GO_DOWN:
next_state = GOING_DOWN;
break;
case STOP:
change_speed(S_DOWN, S_STOP);
next_state = STOP_GOING_DOWN;
delete_orders_at_current_floor(order_table);
open_door();
timer_set();
break;
case OBSTRUCTION:
change_speed(S_DOWN, S_STOP);
activate_emergency(order_table);
next_state = EMERGENCY;
break;
}
return next_state;
}
int handle_event_idle(int event, int order_table[N_FLOORS][N_BUTTONS]) {
int next_state = IDLE;
switch (event) {
case EMERGENCY_BUTTON:
activate_emergency(order_table);
next_state = EMERGENCY;
break;
case GO_UP:
close_door();
change_speed(S_STOP, S_UP);
next_state = GOING_UP;
break;
case GO_DOWN:
close_door();
change_speed(S_STOP, S_DOWN);
next_state = GOING_DOWN;
break;
case STOP:
next_state = IDLE;
break;
case OBSTRUCTION:
activate_emergency(order_table);
next_state = EMERGENCY;
break;
}
return next_state;
}
int handle_event_boot() {
int next_state = IDLE;
int floor = elev_get_floor_sensor_signal();
printf("\nBOOT IN PROGRESS; PLEASE WAIT...\n");
while(floor == -1){
change_speed(STOP, S_DOWN);
floor = elev_get_floor_sensor_signal();
}
elev_set_floor_indicator(floor);
change_speed(S_DOWN, STOP);
printf("\nELEVATOR READY\n");
return next_state;
}
static int startup(struct LEON_serial * info)
{
unsigned long flags;
if (info->flags & S_INITIALIZED)
return 0;
if (!info->xmit_buf) {
info->xmit_buf = (unsigned char *) get_free_page(GFP_KERNEL);
if (!info->xmit_buf)
return -ENOMEM;
}
save_flags(flags); cli();
/*
* Clear the FIFO buffers and disable them
* (they will be reenabled in change_speed())
*/
change_speed(info);
info->xmit_fifo_size = 1;
leon->uartctrl1 = UCTRL_RE | UCTRL_RI | UCTRL_TE /*| UCTRL_TI*/;
//leon->uartdata1;
if (info->tty)
clear_bit(TTY_IO_ERROR, &info->tty->flags);
info->xmit_cnt = info->xmit_head = info->xmit_tail = 0;
info->flags |= S_INITIALIZED;
restore_flags(flags);
return 0;
}
static int set_termios(struct tty_struct * tty, struct termios * termios,
int channel)
{
int i;
unsigned short old_cflag = tty->termios->c_cflag;
/* If we try to set the state of terminal and we're not in the
foreground, send a SIGTTOU. If the signal is blocked or
ignored, go ahead and perform the operation. POSIX 7.2) */
if ((current->tty == channel) &&
(tty->pgrp != current->pgrp)) {
if (is_orphaned_pgrp(current->pgrp))
return -EIO;
if (!is_ignored(SIGTTOU)) {
(void) kill_pg(current->pgrp,SIGTTOU,1);
return -ERESTARTSYS;
}
}
for (i=0 ; i< (sizeof (*termios)) ; i++)
((char *)tty->termios)[i]=get_fs_byte(i+(char *)termios);
if (IS_A_SERIAL(channel) && tty->termios->c_cflag != old_cflag)
change_speed(channel-64);
/* puting mpty's into echo mode is very bad, and I think under
some situations can cause the kernel to do nothing but
copy characters back and forth. -RAB */
if (IS_A_PTY_MASTER(channel)) tty->termios->c_lflag &= ~ECHO;
return 0;
}
int handle_event_stop_going_down(int event, int order_table[N_FLOORS][N_BUTTONS]) {
int next_state = STOP_GOING_DOWN;
switch (event) {
case EMERGENCY_BUTTON:
activate_emergency(order_table);
next_state = EMERGENCY;
break;
case GO_UP:
if(timer_elapsed() > DOOR_DELAY){
close_door();
change_speed(S_STOP, S_UP);
next_state = GOING_UP;
}else{
next_state = STOP_GOING_UP;
}
break;
case GO_DOWN:
if(timer_elapsed() > DOOR_DELAY){
close_door();
change_speed(S_STOP, S_DOWN);
next_state = GOING_DOWN;
}else{
next_state = STOP_GOING_DOWN;
}
break;
case STOP:
if(timer_elapsed() > DOOR_DELAY){
close_door();
next_state = IDLE;
}else{
next_state = STOP_GOING_DOWN;
}
break;
case OBSTRUCTION:
timer_set();
next_state = STOP_GOING_DOWN;
break;
}
return next_state;
}
bool ldp::pre_change_speed(unsigned int uNumerator, unsigned int uDenominator)
{
string strMsg;
// if this is >= 1X
if (uDenominator == 1)
{
m_uFramesToStallPerFrame = 0; // don't want to stall at all
// if this isn't 0 ...
if (uNumerator > 0)
{
m_uFramesToSkipPerFrame = uNumerator - 1; // show 1, skip the rest
}
// else it's 0, which is illegal (use pause() instead unless the game driver specifically wants to do this, in which case more coding is needed)
else
{
m_uFramesToSkipPerFrame = 0;
printline("ERROR : uNumerator of 0 sent to pre_change_speed, this isn't supported, going to 1X");
}
}
// else if this is < 1X
else if (uNumerator == 1)
{
m_uFramesToSkipPerFrame = 0; // don't want to skip any ...
// protect against divide by zero
if (uDenominator > 0)
{
m_uFramesToStallPerFrame = uDenominator - 1; // show 1, stall for the rest
}
// divide by zero situation
else
{
m_uFramesToStallPerFrame = 0;
printline("ERROR : uDenominator of 0 sent to pre_change_speed, this is undefined, going to 1X");
}
}
// else it's a non-standard speed, so do some kind of error
else
{
strMsg = "ERROR : unsupported speed specified (" + numstr::ToStr(uNumerator) +
"/" + numstr::ToStr(uDenominator) + "), setting to 1X";
uNumerator = uDenominator = 1;
}
bool bResult = change_speed(uNumerator, uDenominator);
if (bResult) strMsg = "Successfully changed ";
else strMsg = "Unable to change ";
strMsg += "speed to " + numstr::ToStr(uNumerator) + "/" + numstr::ToStr(uDenominator) +
"X";
printline(strMsg.c_str());
return bResult;
}
/*
* -------------------------------------------------------------------
* startup ()
*
* various initialization tasks
* -------------------------------------------------------------------
*/
static int startup(struct dz_serial *info)
{
unsigned long page, flags;
unsigned short tmp;
if (info->is_initialized)
return 0;
save_flags(flags);
cli();
if (!info->port) {
if (info->tty)
set_bit(TTY_IO_ERROR, &info->tty->flags);
restore_flags(flags);
return -ENODEV;
}
if (!info->xmit_buf) {
page = get_free_page(GFP_KERNEL);
if (!page) {
restore_flags(flags);
return -ENOMEM;
}
info->xmit_buf = (unsigned char *) page;
}
if (info->tty)
clear_bit(TTY_IO_ERROR, &info->tty->flags);
/* enable the interrupt and the scanning */
tmp = dz_in(info, DZ_CSR);
tmp |= (DZ_RIE | DZ_TIE | DZ_MSE);
dz_out(info, DZ_CSR, tmp);
info->xmit_cnt = info->xmit_head = info->xmit_tail = 0;
/* set up the speed */
change_speed(info);
/* clear the line transmitter buffer
I can't figure out why I need to do this - but
its necessary - in order for the console portion
and the interrupt portion to live happily side by side.
*/
/* clear the line transmitter buffer
I can't figure out why I need to do this - but
its necessary - in order for the console portion
and the interrupt portion to live happily side by side.
*/
info->is_initialized = 1;
restore_flags(flags);
return 0;
}
static void rs_set_termios(struct tty_struct *tty, struct ktermios *old_termios)
{
struct m68k_serial *info = (struct m68k_serial *)tty->driver_data;
change_speed(info, tty);
if ((old_termios->c_cflag & CRTSCTS) &&
!(tty->termios.c_cflag & CRTSCTS))
rs_start(tty);
}
static int startup(struct sci_struct * sci)
{
unsigned long flags;
unsigned long page;
page = get_free_page(GFP_KERNEL);
if (!page)
return -ENOMEM;
save_flags(flags); cli();
if (sci->info->flags & ASYNC_INITIALIZED) {
free_page(page);
restore_flags(flags);
return 0;
}
if (!sci->info->port || !sci->info->type) {
if (sci->info->tty)
set_bit(TTY_IO_ERROR, &sci->info->tty->flags);
free_page(page);
restore_flags(flags);
return 0;
}
if (sci->info->xmit_buf)
free_page(page);
else
sci->info->xmit_buf = (unsigned char *) page;
#ifdef SERIAL_DEBUG_OPEN
printk("starting up ttys%d (irq %d)...", info->line, info->irq);
#endif
if (sci->info->tty)
clear_bit(TTY_IO_ERROR, &sci->info->tty->flags);
sci->info->xmit_cnt = sci->info->xmit_head = sci->info->xmit_tail = 0;
/*
* Set up serial timers...
*/
timer_table[RS_TIMER].expires = jiffies + 2*HZ/100;
timer_active |= 1 << RS_TIMER;
/*
* and set the speed of the serial port
*/
change_speed(sci);
sci->info->flags |= ASYNC_INITIALIZED;
restore_flags(flags);
return 0;
}
static int startup(struct cnxt_serial * info)
{
unsigned long flags;
if (info->flags & S_INITIALIZED)
return 0;
if (!info->xmit_buf) {
info->xmit_buf = (unsigned char *) get_free_page(GFP_KERNEL);
if (!info->xmit_buf)
return -ENOMEM;
}
save_flags(flags); cli();
#if 0
/*
* Clear the FIFO buffers and disable them
* (they will be reenabled in change_speed())
*/
USTCNT = USTCNT_UEN;
info->xmit_fifo_size = 1;
USTCNT = USTCNT_UEN | USTCNT_RXEN | USTCNT_TXEN;
(void)URX;
/*
* Finally, enable sequencing and interrupts
*/
#ifdef USE_INTS
USTCNT = USTCNT_UEN | USTCNT_RXEN |
USTCNT_RX_INTR_MASK | USTCNT_TX_INTR_MASK;
#else
USTCNT = USTCNT_UEN | USTCNT_RXEN | USTCNT_RX_INTR_MASK;
#endif
#endif
if (info->tty)
clear_bit(TTY_IO_ERROR, &info->tty->flags);
info->xmit_cnt = info->xmit_head = info->xmit_tail = 0;
/*
* and set the speed of the serial port
*/
#if 0
change_speed(info);
#endif
info->flags |= S_INITIALIZED;
restore_flags(flags);
return 0;
}
int handle_event_emergency(int event, int order_table[N_FLOORS][N_BUTTONS]) {
int next_state = EMERGENCY;
switch (event) {
case EMERGENCY_BUTTON:
activate_emergency(order_table);
next_state = EMERGENCY;
break;
case GO_UP:
if(elev_get_floor_sensor_signal() != -1 && timer_elapsed() < DOOR_DELAY){ //Elevator will wait for # seconds defined by DOOR_DELAY before continuing when at a floor
return EMERGENCY;
}
deactivate_emergency();
change_speed(S_STOP, S_UP);
next_state = GOING_UP;
break;
case GO_DOWN:
if(elev_get_floor_sensor_signal() != -1 && timer_elapsed() < DOOR_DELAY){ //Elevator will wait for # seconds defined by DOOR_DELAY before continuing when at a floor
return EMERGENCY;
}
deactivate_emergency();
change_speed(S_STOP, S_DOWN);
next_state = GOING_DOWN;
break;
case STOP:
next_state = EMERGENCY;
break;
case OBSTRUCTION:
timer_set();
next_state = EMERGENCY;
break;
}
return next_state;
}
static int startup(struct m68k_serial * info)
{
m68328_uart *uart = &uart_addr[info->line];
unsigned long flags;
if (info->flags & S_INITIALIZED)
return 0;
if (!info->xmit_buf) {
info->xmit_buf = (unsigned char *) __get_free_page(GFP_KERNEL);
if (!info->xmit_buf)
return -ENOMEM;
}
local_irq_save(flags);
/*
* Clear the FIFO buffers and disable them
* (they will be reenabled in change_speed())
*/
uart->ustcnt = USTCNT_UEN;
info->xmit_fifo_size = 1;
uart->ustcnt = USTCNT_UEN | USTCNT_RXEN | USTCNT_TXEN;
(void)uart->urx.w;
/*
* Finally, enable sequencing and interrupts
*/
#ifdef USE_INTS
uart->ustcnt = USTCNT_UEN | USTCNT_RXEN |
USTCNT_RX_INTR_MASK | USTCNT_TX_INTR_MASK;
#else
uart->ustcnt = USTCNT_UEN | USTCNT_RXEN | USTCNT_RX_INTR_MASK;
#endif
if (info->tty)
clear_bit(TTY_IO_ERROR, &info->tty->flags);
info->xmit_cnt = info->xmit_head = info->xmit_tail = 0;
/*
* and set the speed of the serial port
*/
change_speed(info);
info->flags |= S_INITIALIZED;
local_irq_restore(flags);
return 0;
}
/*
* This routine is called whenever a serial port is opened. It
* enables interrupts for a serial port, linking in its S structure into
* the IRQ chain. It also performs the serial-specific
* initialization for the tty structure.
*/
int rs_open(struct tty_struct *tty, struct file * filp)
{
int retval, line;
struct cnxt_serial *info;
line = MINOR(tty->device) - tty->driver.minor_start;
if (line != 0) /* we have exactly one */
return -ENODEV;
info = &uart_info;
if (serial_paranoia_check(info, tty->device, "rs_open"))
return -ENODEV;
info->count++;
tty->driver_data = info;
info->tty = tty;
/*
* Start up serial port
*/
retval = startup(info);
if (retval)
return retval;
retval = block_til_ready(tty, filp, info);
if (retval) {
printk("rs_open returning after block_til_ready with %d\n",
retval);
return retval;
}
if ((info->count == 1) && (info->flags & S_SPLIT_TERMIOS)) {
if (tty->driver.subtype == SERIAL_TYPE_NORMAL)
*tty->termios = info->normal_termios;
else
*tty->termios = info->callout_termios;
change_speed(info);
}
info->session = current->session;
info->pgrp = current->pgrp;
// Enable GPIO interrupt line for console uart
SetGPIOIntEnable(GPIOINT_UART1, IRQ_ON);
return 0;
}
static void rs_set_termios(struct tty_struct *tty, struct ktermios *old_termios)
{
struct serial_state *info = tty->driver_data;
unsigned long flags;
unsigned int cflag = tty->termios->c_cflag;
change_speed(tty, info, old_termios);
/* Handle transition to B0 status */
if ((old_termios->c_cflag & CBAUD) &&
!(cflag & CBAUD)) {
info->MCR &= ~(SER_DTR|SER_RTS);
local_irq_save(flags);
rtsdtr_ctrl(info->MCR);
local_irq_restore(flags);
}
/* Handle transition away from B0 status */
if (!(old_termios->c_cflag & CBAUD) &&
(cflag & CBAUD)) {
info->MCR |= SER_DTR;
if (!(tty->termios->c_cflag & CRTSCTS) ||
!test_bit(TTY_THROTTLED, &tty->flags)) {
info->MCR |= SER_RTS;
}
local_irq_save(flags);
rtsdtr_ctrl(info->MCR);
local_irq_restore(flags);
}
/* Handle turning off CRTSCTS */
if ((old_termios->c_cflag & CRTSCTS) &&
!(tty->termios->c_cflag & CRTSCTS)) {
tty->hw_stopped = 0;
rs_start(tty);
}
#if 0
/*
* No need to wake up processes in open wait, since they
* sample the CLOCAL flag once, and don't recheck it.
* XXX It's not clear whether the current behavior is correct
* or not. Hence, this may change.....
*/
if (!(old_termios->c_cflag & CLOCAL) &&
(tty->termios->c_cflag & CLOCAL))
wake_up_interruptible(&info->open_wait);
#endif
}
/* FIX UP modem control here someday......
*/
static void rs_360_set_termios(struct tty_struct *tty, struct ktermios *old_termios)
{
ser_info_t *info = (ser_info_t *)tty->driver_data;
change_speed(info);
#ifdef modem_control
/* Handle transition to B0 status */
if ((old_termios->c_cflag & CBAUD) &&
!(tty->termios->c_cflag & CBAUD)) {
info->MCR &= ~(UART_MCR_DTR|UART_MCR_RTS);
local_irq_disable();
serial_out(info, UART_MCR, info->MCR);
local_irq_enable();
}
/* Handle transition away from B0 status */
if (!(old_termios->c_cflag & CBAUD) &&
(tty->termios->c_cflag & CBAUD)) {
info->MCR |= UART_MCR_DTR;
if (!tty->hw_stopped ||
!(tty->termios->c_cflag & CRTSCTS)) {
info->MCR |= UART_MCR_RTS;
}
local_irq_disable();
serial_out(info, UART_MCR, info->MCR);
local_irq_enable();
}
/* Handle turning off CRTSCTS */
if ((old_termios->c_cflag & CRTSCTS) &&
!(tty->termios->c_cflag & CRTSCTS)) {
tty->hw_stopped = 0;
rs_360_start(tty);
}
#endif
#if 0
/*
* No need to wake up processes in open wait, since they
* sample the CLOCAL flag once, and don't recheck it.
* XXX It's not clear whether the current behavior is correct
* or not. Hence, this may change.....
*/
if (!(old_termios->c_cflag & CLOCAL) &&
(tty->termios->c_cflag & CLOCAL))
wake_up_interruptible(&info->open_wait);
#endif
}
static void rs_set_termios(struct tty_struct *tty, struct termios *old_termios)
{
struct LEON_serial *info = (struct LEON_serial *)tty->driver_data;
if (tty->termios->c_cflag == old_termios->c_cflag)
return;
change_speed(info);
if ((old_termios->c_cflag & CRTSCTS) &&
!(tty->termios->c_cflag & CRTSCTS)) {
tty->hw_stopped = 0;
rs_start(tty);
}
}
/*
* This only works as the 386 is low-byte-first
*/
static int set_termio(struct tty_struct * tty, struct termio * termio,
int channel)
{
int i;
struct termio tmp_termio;
unsigned short old_cflag = tty->termios->c_cflag;
if ((current->tty == channel) &&
(tty->pgrp > 0) &&
(tty->pgrp != current->pgrp)) {
if (is_orphaned_pgrp(current->pgrp))
return -EIO;
if (!is_ignored(SIGTTOU)) {
(void) kill_pg(current->pgrp,SIGTTOU,1);
return -ERESTARTSYS;
}
}
for (i=0 ; i< (sizeof (*termio)) ; i++)
((char *)&tmp_termio)[i]=get_fs_byte(i+(char *)termio);
/* take care of the packet stuff. */
if ((tmp_termio.c_iflag & IXON) &&
~(tty->termios->c_iflag & IXON))
{
tty->status_changed = 1;
tty->ctrl_status |= TIOCPKT_DOSTOP;
}
if (~(tmp_termio.c_iflag & IXON) &&
(tty->termios->c_iflag & IXON))
{
tty->status_changed = 1;
tty->ctrl_status |= TIOCPKT_NOSTOP;
}
*(unsigned short *)&tty->termios->c_iflag = tmp_termio.c_iflag;
*(unsigned short *)&tty->termios->c_oflag = tmp_termio.c_oflag;
*(unsigned short *)&tty->termios->c_cflag = tmp_termio.c_cflag;
*(unsigned short *)&tty->termios->c_lflag = tmp_termio.c_lflag;
tty->termios->c_line = tmp_termio.c_line;
for(i=0 ; i < NCC ; i++)
tty->termios->c_cc[i] = tmp_termio.c_cc[i];
if (IS_A_SERIAL(channel) && tty->termios->c_cflag != old_cflag)
change_speed(channel-64);
return 0;
}
static void rs_set_termios(struct tty_struct *tty, struct termios *old_termios)
{
struct async_struct *info = (struct async_struct *)tty->driver_data;
if ( (tty->termios->c_cflag == old_termios->c_cflag)
&& ( RELEVANT_IFLAG(tty->termios->c_iflag)
== RELEVANT_IFLAG(old_termios->c_iflag)))
return;
change_speed(info);
if ((old_termios->c_cflag & CRTSCTS) &&
!(tty->termios->c_cflag & CRTSCTS)) {
tty->hw_stopped = 0;
rs_start(tty);
}
}
/*
* This routine is called whenever a serial port is opened. It
* enables interrupts for a serial port. It also performs the
* serial-specific initialization for the tty structure.
*/
static int dz_open(struct tty_struct *tty, struct file *filp)
{
struct dz_serial *info;
int retval, line;
line = MINOR(tty->device) - tty->driver.minor_start;
/* The dz lines for the mouse/keyboard must be
* opened using their respective drivers.
*/
if ((line < 0) || (line >= DZ_NB_PORT))
return -ENODEV;
if ((line == DZ_KEYBOARD) || (line == DZ_MOUSE))
return -ENODEV;
info = lines[line];
info->count++;
tty->driver_data = info;
info->tty = tty;
/*
* Start up serial port
*/
retval = startup(info);
if (retval)
return retval;
retval = block_til_ready(tty, filp, info);
if (retval)
return retval;
if ((info->count == 1) && (info->flags & DZ_SPLIT_TERMIOS)) {
if (tty->driver.subtype == SERIAL_TYPE_NORMAL)
*tty->termios = info->normal_termios;
else
*tty->termios = info->callout_termios;
change_speed(info);
}
info->session = current->session;
info->pgrp = current->pgrp;
return 0;
}
static void
am_uart_set_termios(struct uart_port *port, struct ktermios *termios,
struct ktermios *old)
{
unsigned int baud;
struct am_uart_port * info = &am_ports[port->line];
#ifdef PRINT_DEBUG
if(info->line == DEBUG_PORT_ID)
printk("%s\n", __FUNCTION__);
#endif
baud = tty_termios_baud_rate(termios);
change_speed(info, baud);
uart_update_timeout(port, termios->c_cflag, baud);
return;
}
static int set_termios(struct tty_struct * tty, struct termios * termios,
int channel)
{
int i, retsig;
/* If we try to set the state of terminal and we're not in the
foreground, send a SIGTTOU. If the signal is blocked or
ignored, go ahead and perform the operation. POSIX 7.2) */
if ((current->tty == channel) && (tty->pgrp != current->pgrp)) {
retsig = tty_signal(SIGTTOU, tty);
if (retsig == -ERESTARTSYS || retsig == -EINTR)
return retsig;
}
for (i=0 ; i< (sizeof (*termios)) ; i++)
((char *)&tty->termios)[i]=get_fs_byte(i+(char *)termios);
change_speed(tty);
return 0;
}
/*
* This routine is called whenever a serial port is opened. It
* enables interrupts for a serial port, linking in its S structure into
* the IRQ chain. It also performs the serial-specific
* initialization for the tty structure.
*/
int rs_open(struct tty_struct *tty, struct file * filp)
{
struct NIOS_serial *info;
int retval, line;
line = MINOR(tty->device) - tty->driver.minor_start;
if (line >=serial_driver.num) /* too many */
return -ENODEV;
info = &nios_soft[line];
if (serial_paranoia_check(info, tty->device, "rs_open"))
return -ENODEV;
info->count++;
tty->driver_data = info;
info->tty = tty;
/*
* Start up serial port
*/
retval = startup(info);
if (retval)
return retval;
retval = block_til_ready(tty, filp, info);
if (retval) {
return retval;
}
if ((info->count == 1) && (info->flags & S_SPLIT_TERMIOS)) {
if (tty->driver.subtype == SERIAL_TYPE_NORMAL)
*tty->termios = info->normal_termios;
else
*tty->termios = info->callout_termios;
change_speed(info);
}
info->session = current->session;
info->pgrp = current->pgrp;
return 0;
}
static int startup(struct async_struct * info)
{
unsigned long flags;
unsigned long page;
page = get_free_page(GFP_KERNEL);
if (!page)
return -ENOMEM;
save_flags(flags); cli();
if (info->flags & ASYNC_INITIALIZED) {
free_page(page);
restore_flags(flags);
return 0;
}
if (info->xmit_buf)
free_page(page);
else
info->xmit_buf = (unsigned char *) page;
#ifdef SERIAL_DEBUG_OPEN
printk("starting up ttys%d (irq %d)...", info->line, info->irq);
#endif
if (info->tty)
clear_bit(TTY_IO_ERROR, &info->tty->flags);
info->xmit_cnt = info->xmit_head = info->xmit_tail = 0;
/*
* and set the speed of the serial port
*/
change_speed(info);
info->flags |= ASYNC_INITIALIZED;
restore_flags(flags);
return 0;
}
static void rs_set_termios(struct tty_struct *tty, struct termios *old_termios)
{
struct NIOS_serial *info = (struct NIOS_serial *)tty->driver_data;
int oldbaud = info->baud;
if (tty->termios->c_cflag == old_termios->c_cflag)
return;
change_speed(info);
if (info->baud == oldbaud) /* can not change */
tty->termios->c_cflag = old_termios->c_cflag;
else /* only speed can be changed */
tty->termios->c_cflag = (tty->termios->c_cflag & CBAUD) | CS8 | CREAD | HUPCL | CLOCAL;
#if 0
if ((old_termios->c_cflag & CRTSCTS) &&
!(tty->termios->c_cflag & CRTSCTS)) {
tty->hw_stopped = 0;
rs_start(tty);
}
#endif
}
task main ()
{
// call initialization functions
init_flappy_wing() ;
init_drive_parameters() ;
// loop forever
while (1)
{
// call drive routines
change_speed();
change_drive_direction();
drive();
// call collection and flappy wing routines
collection();
flappy_wing();
// call arm routines
arm_joint();
end_effector();
}
}
void turn(t_lidar *lidar)
{
if (lidar->left - lidar->right > 0 && lidar->middle_right <= 700)
{
if (lidar->wheels + 0.1 <= 0.4)
lidar->wheels += 0.1;
wheels(lidar->wheels, lidar);
if (lidar->speed - 0.1 >= 0.2)
lidar->speed -= 0.1;
forward(lidar->speed, lidar);
}
else if (lidar->left - lidar->right < 0 && lidar->middle_left <= 700)
{
if (lidar->wheels - 0.1 >= -0.4)
lidar->wheels -= 0.1;
wheels(lidar->wheels, lidar);
if (lidar->speed - 0.1 >= 0.2)
lidar->speed -= 0.1;
forward(lidar->speed, lidar);
}
else
change_speed(lidar);
}
/*
* This only works as the 386 is low-byt-first
*/
static int set_termio(struct tty_struct * tty, struct termio * termio,
int channel)
{
int i, retsig;
struct termio tmp_termio;
if ((current->tty == channel) && (tty->pgrp != current->pgrp)) {
retsig = tty_signal(SIGTTOU, tty);
if (retsig == -ERESTARTSYS || retsig == -EINTR)
return retsig;
}
for (i=0 ; i< (sizeof (*termio)) ; i++)
((char *)&tmp_termio)[i]=get_fs_byte(i+(char *)termio);
*(unsigned short *)&tty->termios.c_iflag = tmp_termio.c_iflag;
*(unsigned short *)&tty->termios.c_oflag = tmp_termio.c_oflag;
*(unsigned short *)&tty->termios.c_cflag = tmp_termio.c_cflag;
*(unsigned short *)&tty->termios.c_lflag = tmp_termio.c_lflag;
tty->termios.c_line = tmp_termio.c_line;
for(i=0 ; i < NCC ; i++)
tty->termios.c_cc[i] = tmp_termio.c_cc[i];
change_speed(tty);
return 0;
}
static int startup(struct NIOS_serial * info)
{
unsigned long flags;
np_uart * uart= (np_uart *)(info->port);
if (info->flags & S_INITIALIZED)
return 0;
if (!info->xmit_buf) {
info->xmit_buf = (unsigned char *) get_free_page(GFP_KERNEL);
if (!info->xmit_buf)
return -ENOMEM;
}
save_flags(flags); cli();
/*
* Clear the FIFO buffers and disable them
* (they will be reenabled in change_speed())
*/
change_speed(info);
info->xmit_fifo_size = 1;
uart->np_uartcontrol = np_uartcontrol_itrdy_mask
| np_uartcontrol_irrdy_mask
| np_uartcontrol_ibrk_mask;
uart->np_uartrxdata;
if (info->tty)
clear_bit(TTY_IO_ERROR, &info->tty->flags);
info->xmit_cnt = info->xmit_head = info->xmit_tail = 0;
info->flags |= S_INITIALIZED;
restore_flags(flags);
return 0;
}
static void
pdc_set_termios(struct tty_struct *tty, struct termios *old_termios)
{
#if 0 /* XXX CP, has to be checked, if there is stuff to do */
struct async_struct *info = (struct async_struct *) tty->driver_data;
unsigned long flags;
unsigned int cflag = tty->termios->c_cflag;
if ((cflag == old_termios->c_cflag)
&& (RELEVANT_IFLAG(tty->termios->c_iflag)
== RELEVANT_IFLAG(old_termios->c_iflag)))
return;
#if 0
change_speed(info, old_termios);
#endif
/* Handle turning off CRTSCTS */
if ((old_termios->c_cflag & CRTSCTS) &&
!(tty->termios->c_cflag & CRTSCTS)) {
tty->hw_stopped = 0;
pdc_start(tty);
}
#endif
}
