void tx232Isr (){
while (BusyUSART()){}
TXREG = *txptr;
txptr++;
if (TXREG == 0)
disable_tx();
}
void send_rx_buffer() {
rx.val.pID = SERVO_ADDR;
uint8_t cs1 = calculate_cs1();
rx.val.cs1 = (cs1 & 0xFE);
rx.val.cs2 = ((cs1 ^ 0xFF) & 0xFE);
enable_tx();
hwuart_tx(0xff); // sync
hwuart_tx(0xff);
uint8_t tosend = rx.val.size - 2;
for (uint8_t i=0; i < tosend; i++) {
hwuart_tx(rx.rawdata[i]);
}
hwuart_tx(0); // junk, just in case
disable_tx();
}
static int bladerf_release(struct inode *inode, struct file *file)
{
bladerf_device_t *dev;
dev = (bladerf_device_t *)file->private_data;
if (dev->writer && dev->writer == file) {
if (dev->tx_en) {
disable_tx(dev);
}
dev->writer = NULL;
}
if (dev->reader && dev->reader == file) {
if (dev->rx_en) {
disable_rx(dev);
}
dev->reader = NULL;
}
return 0;
}
int main(void) {
INIT_SYSTEM_CLOCK();
// Set up ports. Enable pullups on all unused inputs.
DDRB = (1<<1) | (1<<2) | (1<<5);
PORTB = ~DDRB;
DDRC = 0;
PORTC = ~DDRC;
// Do not enable TX for now, enable serial bypass.
DDRD = (1<<2) | (1<<4) | (1<<6);
// No pullups on serial RX, pullups on all other inputs
PORTD = ~(DDRD | (1<<0));
// set up timer/counter 0 to overflow every ~10mS (in CTC mode)
TCCR0A = (1<<WGM01);
TIMSK0 = 0; // no interrupts, will check flag by hand.
#if (F_CPU == 16000000)
TCCR0B = (1<<CS02)|(1<<CS00); // Prescaler 1/1024
OCR0A = 156; // 10.048 mS interval
#else
#error Unsupported CPU speed
#endif
// set up t/c 1 as phase-correct 8-bit non-inverted PWM on both channels
// prescaler is 4, which works out to 4kHz @ 16 MHz clock
// (as motor boards can only take up to 10kHz PWM frequency)
OCR1A = 0;
OCR1B = 0;
TCCR1A = (1<<COM1A1) | (1<<COM1B1) | (1<<WGM10);
TCCR1B = (1<<CS11);
hwuart_init();
hwuart_connect_stdout();
disable_tx();
sei();
uint16_t step = 0;
while (1) {
if (rx_status == RXS_READY) {
if (!verify_rx_checksum()) {
servo_status_err |= kSSEInvalidPacket;
servo_status_det |= kSSDChecksumError;
} else if (rx.val.cmd == 0x09) { // REBOOT
servo_status_err = 0;
servo_status_det = 0;
servo_leds = 0;
servo_fire_time = 0;
servo_fire_pwm = 0;
servo_agitator_pwm = 0;
apply_servo_values();
} else if (rx.val.cmd == 0x07) { // STAT
handle_STAT_command();
} else if (rx.val.cmd == 0x03 &&
rx.val.size >= 8) { // RAM WRITE + at least 1 byte
handle_RAMWRITE_cmd();
// Do not apply servo values now (to maintain uniform interval)
} else if (rx.val.cmd == 0x04 &&
rx.val.size == 9) { // RAM READ + 2 data bytes
handle_RAMREAD_cmd();
} else if (rx.val.cmd == 0x05 || // I_JOG
rx.val.cmd == 0x06 || // S_JOG
rx.val.cmd == 0x08) { // ROLLBACK
; // silently ignore
} else {
servo_status_err |= kSSEInvalidPacket;
servo_status_det |= kSSDUnknownCommand;
}
rx_status = RXS_BAD;
};
if (!(TIFR0 & (1<<OCF0A))) {
// Timer not elapsed yet. Spin more.
continue;
}
TIFR0 = (1<<OCF0A);
// ~10mS has elapsed.
// expire LEDs
if (servo_leds_timeout) {
servo_leds_timeout--;
} else {
servo_leds = 0;
servo_agitator_pwm = 0;
}
apply_servo_values();
// expire fire timer
if (servo_fire_time) {
servo_fire_time--;
} else {
servo_fire_pwm = 0;
servo_status_det &= ~0x01; // MOVING flag
}
step++;
// blink LED only if it is not explicilty set
// turn it on for 100mS every 10 seconds
if (!servo_leds_timeout) {
if (step == 10) {
LED_BLUE = 0;
} else if (step >= 1000) {
LED_BLUE = 1;
step = 0;
}
}
/*
if ((i % 64) == 2) {
enable_tx();
printf_PSTR("SERIAL TEST %d ST=%d LC=%d LEN=%d CMD=%d\n",
i, rx_status, rx_len, rx.val.cmd);
disable_tx();
}
*/
}
}