/**
* Extension of print_time to print the time along with the .s for timer mode
* @param d device to print to
* @param time time to print
*/
void print_time_ex(device_t d, unsigned int time) {
static char tmp[9];
time_to_string(time, tmp);
dputs(d, tmp);
dputchar(d, '.');
dputchar(d, '0' + (time/100)%10);
}
/**
* Extension of print_time_diff to add on the .s for timer mode
* @param d device to print to
* @param cur current time
* @param last the last time to be printed on the screen
*/
void print_time_diff_ex(device_t d, unsigned int cur, unsigned int last) {
if (last/1000 != cur/1000) {
dputs(d, "");
print_time_diff(d, cur, last);
dputchar(d, '.');
dputchar(d, '0' + (cur/100)%10);
}
else if ((last/100)%10 != (cur/100)%10) {
dputchar(d, '');
dputchar(d, '0' + (cur/100)%10);
}
}
void dputs(const char *str)
{
while(*str) {
dputchar(*str);
str++;
}
}
// This function sends a Null-terminates String via dputchar().
// It will return the number of transmitted chars.
int debug( const char *s ) {
int i = 0;
while( *s != 0 ) {
dputchar( *s++ );
i++;
}
return i;
}
/*
* Pointers to local automatic storage must be far when routine is
* called from interrupt level.
*/
void
/* dputs(char far *p) */
dputs(char *p)
{
while(*p != '\0') {
dputchar(*p++);
}
}
void tpu_wait_idle(void)
{
dputs("Waiting for TPU Idle ");
/* Wait until TPU is doing something */
delay_us(3);
/* Wait until TPU is idle */
while (readw(TPU_REG(TPU_CTRL)) & TPU_CTRL_IDLE)
dputchar('.');
dputs("Done!\n");
}
signed short int find_next_j(signed short int* stat)
{
dprintf("find_next_j: ");
for(; *stat<Ceilings; ++*stat){
dprintf("[%d]%d ", *stat, dned[*stat]);
if(!dned[*stat]){
dputchar('\n');
return *stat;
}
}
return Ceilings;
}
/* Entry point for interrupts */
void irq(void)
{
uint8_t num, tmp;
irq_handler *handler;
#if 1
/* Hardware interrupt detection mode */
num = readb(IRQ_REG(IRQ_NUM)) & 0x1f;
printd("i%02x\n", num);
handler = irq_handlers[num];
if (handler)
handler(num);
#else
/* Software interrupt detection mode */
{
uint16_t it_reg, mask_reg;
uint32_t irqs;
it_reg = readw(IRQ_REG(IT_REG1));
mask_reg = readw(IRQ_REG(MASK_IT_REG1));
irqs = it_reg & ~mask_reg;
it_reg = readw(IRQ_REG(IT_REG2));
mask_reg = readw(IRQ_REG(MASK_IT_REG2));
irqs |= (it_reg & ~mask_reg) << 16;
for (num = 0; num < 32; num++) {
if (irqs & (1 << num)) {
printd("i%d\n", num);
handler = irq_handlers[num];
if (handler)
handler(num);
/* clear this interrupt */
if (num < 16)
writew(~(1 << num), IRQ_REG(IT_REG1));
else
writew(~(1 << (num-16)), IRQ_REG(IT_REG2));
}
}
dputchar('\n');
}
#endif
/* Start new IRQ agreement */
tmp = readb(IRQ_REG(IRQ_CTRL));
tmp |= 0x01;
writeb(tmp, IRQ_REG(IRQ_CTRL));
}
int i2c_write(uint8_t chip, uint32_t addr, int alen, const uint8_t *buffer, int len)
{
uint8_t cmd;
/* Calypso I2C controller doesn't support fancy addressing */
if (alen > 1)
return -1;
/* FIXME: implement writes longer than fifo size */
if (len > 16)
return -1;
printd("i2c_write(chip=0x%02u, addr=0x%02u): ", chip, addr)
writeb(chip & 0x3f, I2C_REG(DEVICE_REG));
writeb(addr & 0xff, I2C_REG(ADDRESS_REG));
/* we have to tell the controller how many bits we'll put into the fifo ?!? */
writeb(len-1, I2C_REG(CONF_FIFO_REG));
/* fill the FIFO */
while (len--) {
uint8_t byte = *buffer++;
writeb(byte, I2C_REG(DATA_WR_REG));
printd("%02X ", byte);
}
dputchar('\n');
/* start the transfer */
cmd = readb(I2C_REG(CMD_REG));
cmd |= I2C_CMD_START;
writeb(cmd, I2C_REG(CMD_REG));
/* wait until transfer completes */
while (1) {
uint8_t reg = readb(I2C_REG(STATUS_ACTIVITY_REG));
printd("I2C Status: 0x%02x\n", rerg & 0xf);
if (!(reg & I2C_STATUS_IDLE)) // 0: idle 1: not idle
break;
}
dputs("I2C transfer completed\n");
return 0;
}
// send the full list of commands to the servo board with a 2ms delay between chars
void sendCommands(const int& controlX, const int& controlY, int fd) {
// Temp value for pulling characters out of command
int intTemp;
static char digits[10] = {'0','1','2','3','4','5','6','7','8','9'};
static char esc = (unsigned char) 27;
// Send esc
dputchar(esc,fd);
// Sending roll info
dputchar('p',fd);
intTemp = controlX / 100 % 10;
dputchar(digits[intTemp],fd);
intTemp = controlX / 10 % 10;
dputchar(digits[intTemp],fd);
intTemp = controlX % 10;
dputchar(digits[intTemp],fd);
// Sending pitch info
dputchar('r', fd);
intTemp = controlY / 100 % 10;
dputchar(digits[intTemp],fd);
intTemp = controlY / 10 % 10;
dputchar(digits[intTemp],fd);
intTemp = controlY % 10;
dputchar(digits[intTemp],fd);
}
/**
* Calculate difference between two times given and print out
* the required digits. Also takes care of backspacing flags
* before printing
* @param device device to print to
* @param current current time
* @param last last time printed to screen to be diffed against
*/
void print_time_diff(unsigned int device, unsigned int current, unsigned int last)
{
//This is the digit scheme used in variable names below:
// h2h1:m2m1:s2s1
int diff = 0;
//Calculate current digits because they are always possibly needed
unsigned int current_minutes = (current/MS_PER_MIN)%60;
int current_minutes_m1 = current_minutes%10;
int current_minutes_m2 = current_minutes/10;
unsigned int current_seconds = (current/MS_PER_SEC)%60;
int current_seconds_s1 = current_seconds%10;
int current_seconds_s2 = current_seconds/10;
unsigned int current_hours = (current/MS_PER_HOUR)%24;
int current_hours_h1 = current_hours%10;
int current_hours_h2 = current_hours/10;
//Calculate Hours Digits
unsigned int last_hours = (last/MS_PER_HOUR)%24;
int last_hours_h1 = last_hours%10;
int last_hours_h2 = last_hours/10;
//short circuit if different
if(current_hours_h2 - last_hours_h2)
diff = 8;
else if(current_hours_h1 - last_hours_h1)
diff = 7;
if(!diff)
{
//Calculate Minutes digits
unsigned int last_minutes = (last/MS_PER_MIN)%60;
int last_minutes_m1 = last_minutes%10;
int last_minutes_m2 = last_minutes/10;
//short circuit if different
if(current_minutes_m2 - last_minutes_m2)
diff = 5;
else if(current_minutes_m1 - last_minutes_m1)
diff = 4;
}
if(!diff)
{
//Calculate Seconds digits
unsigned int last_seconds = (last/MS_PER_SEC)%60;
int last_seconds_s1 = last_seconds%10;
int last_seconds_s2 = last_seconds/10;
//short circuit if different
if(current_seconds_s2 - last_seconds_s2)
diff = 2;
else if(current_seconds_s1 - last_seconds_s1)
diff = 1;
}
//Diff now has the proper value
for(int i = 0; i < diff; i++)
{
dputchar(device, '');
}
switch(diff)
{
case 8:
dputchar(device, (char)(current_hours_h2 + '0'));
case 7:
dputchar(device, (char)(current_hours_h1 + '0'));
case 6:
dputchar(device, ':');
case 5:
dputchar(device, (char)(current_minutes_m2 + '0'));
case 4:
dputchar(device, (char)(current_minutes_m1 + '0'));
case 3:
dputchar(device, ':');
case 2:
dputchar(device, (char)(current_seconds_s2 + '0'));
case 1:
dputchar(device, (char)(current_seconds_s1 + '0'));
}
}
