uint16_t AsebaGetBuffer(AsebaVMState *vm, uint8_t * data, uint16_t maxLength, uint16_t* source) { int flags; uint16_t ret = 0; size_t u; // Touching the FIFO, mask the interrupt ... USBMaskInterrupts(flags); u = get_used(&AsebaUsb.rx); /* Minium packet size == len + src + msg_type == 6 bytes */ if(u >= 6) { int len; fifo_peek((unsigned char *) &len, &AsebaUsb.rx, 2); if (u >= len + 6) { memcpy_out_fifo((unsigned char *) &len, &AsebaUsb.rx, 2); memcpy_out_fifo((unsigned char *) source, &AsebaUsb.rx, 2); // msg_type is not in the len but is always present len = len + 2; /* Yay ! We have a complete packet ! */ if(len > maxLength) len = maxLength; memcpy_out_fifo(data, &AsebaUsb.rx, len); ret = len; } } if(usb_uart_serial_port_open()) USBCDCKickRx(); else fifo_reset(&AsebaUsb.rx); USBUnmaskInterrupts(flags); return ret; }
void AsebaSendBuffer(AsebaVMState *vm, const uint8_t *data, uint16_t length) { int flags; // Here we must loop until we can send the data. // BUT if the usb connection is not available, we drop the packet if(!usb_uart_serial_port_open()) return; // Sanity check, should never be true if (length < 2) return; do { USBMaskInterrupts(flags); if(get_free(&AsebaUsb.tx) > length + 4) { length -= 2; memcpy_to_fifo(&AsebaUsb.tx, (unsigned char *) &length, 2); memcpy_to_fifo(&AsebaUsb.tx, (unsigned char *) &vm->nodeId, 2); memcpy_to_fifo(&AsebaUsb.tx, (unsigned char *) data, length + 2); // Will callback AsebaUsbTxReady if (!tx_busy) { tx_busy = 1; USBCDCKickTx(); } length = 0; } // Usb can be disconnected while sending ... if(!usb_uart_serial_port_open()) { fifo_reset(&AsebaUsb.tx); USBUnmaskInterrupts(flags); break; } USBUnmaskInterrupts(flags); } while(length); }
int main(void) { int test_mode; int vm_present; int i; unsigned int seed; // Needs to be called ASAP as rf need a looooooong time to wake up. // This function is just sending a pulse over the SCL line. rf_wakeup(); clock_set_speed(16000000UL,16); setup_pps(); setup_io(); leds_init(); CHARGE_500MA = 0; // Switch back to 100mA charge. // Switch on one led to say we are powered on leds_set(LED_BATTERY_0, 32); // Enable the poweroff softirq. _INT3IF = 0; _INT3IP = 1; _INT3IE = 1; // Sound must be enabled before analog, as // The analog interrupt callback into sound processing ... // But must be initialised _after_ leds as it use one led IO for enabling amp. sound_init(); tone_init(); // Init tone generator pwm_motor_init(); pid_motor_init(); // We need the settings for the horizontal prox. load_settings_from_flash(); for (i = 0; i < 2; i++) { // Settings is definitely wrong.... if(settings.mot256[i] <= 0) settings.mot256[i] = 256; // 1024 (AD resolution is 10 bits) * 256 / 9 fits in signed 16 bits. if (settings.mot256[i] < 9) settings.mot256[i] = 9; } // This is the horizontal prox. Vertical one are handled by the ADC // but ADC sync the motor mesurment with the prox, so we don't pullute it with noise ... timer_init(TIMER_IR_COMM, 0,-1); // The period will be changed later. prox_init(PRIO_SENSORS); // Same priority as analog (maybe should be at 7 ...) // Warning: We cannot use the SD before the analog init as some pin are on the analog port. analog_init(TIMER_ANALOG, PRIO_SENSORS); wait_valid_vbat(); log_init(); // We will need to read vbat to be sure we can flash. ntc_init(ntc_callback, PRIO_1KHZ); // i2c_init(I2C_3); i2c_init_master(I2C_3, 400000, PRIO_I2C); I2C3CON = 0x9000; mma7660_init(I2C_3, MMA7660_DEFAULT_ADDRESS, acc_cb, 0); mma7660_set_mode(MMA7660_120HZ, 1); rc5_init(TIMER_RC5, rc5_callback, PRIO_RC5); sd_init(); timer_init(TIMER_1KHZ, 1000, 6); timer_enable_interrupt(TIMER_1KHZ, timer_1khz, PRIO_1KHZ); rf_init(I2C_3); timer_enable(TIMER_1KHZ); sd_log_file(); vm_present = init_aseba_and_fifo(); if(vm_present) log_analyse_bytecode(); vmVariables.fwversion[0] = FW_VERSION; vmVariables.fwversion[1] = FW_VARIANT; // SD file is more important than internal flash if(!sd_load_aseba_code()) { log_set_flag(LOG_FLAG_VMCODESD); vm_present = 1; log_analyse_bytecode(); } // Behavior is on INT4 (softirq trigged by 1khz timer). behavior_init(PRIO_BEHAVIOR); test_mode = sd_test_file_present(); if(!test_mode) mode_init(vm_present); // Enable the LVD interrupt _LVDIE = 1; play_sound(SOUND_POWERON); if(test_mode) { test_mode_start(); while(1) idle_without_aseba(); } while(behavior_enabled(B_MODE)) idle_without_aseba(); // If usb did not put us out of behavior mode, then start the rf link if(!usb_uart_serial_port_open() && (rf_get_status() & RF_PRESENT)) { rf_set_link(RF_UP); } // get the random seed seed = 0; for(i = 0; i < 5; i++) { seed += vmVariables.buttons_mean[i]; seed += vmVariables.buttons_noise[i]; } seed += vmVariables.vbat[0]; seed += vmVariables.vbat[1]; for(i = 0; i < 3; i++) seed += vmVariables.acc[i]; AsebaSetRandomSeed(seed); for(i = 0; i < 3; i++) AsebaGetRandom(); // Give full control to aseba. No way out (except reset). run_aseba_main_loop(); }