void run_translatorI2C() { unsigned char mod, reg, val; uart_send_static_text("\f\a" "WELCOME to the RS232 - I2C translator on e-Puck\r\n" " 1 byte protocol ASL-EPFL 2006\r\n"); e_i2cp_init(); while (1) { e_getchar_uart1(&mod); // #module mod=(unsigned char) (mod<<1); e_getchar_uart1(®); // #register if (reg>127) { // read e_i2cp_enable(); val= e_i2cp_read(mod, reg&0x7f); // read I2C e_i2cp_disable(); e_send_uart1_char(&val, 1); } else { // write e_getchar_uart1(&val); // #value e_i2cp_enable(); e_i2cp_write(mod, reg, val); // write I2C e_i2cp_disable(); } } }
void el_disable_camera(){ if(el_cam_enabled){ IEC0bits.T1IE = 0; IEC1bits.T4IE = 0; IEC1bits.T5IE = 0; T1CONbits.TON = 0; T4CONbits.TON = 0; T5CONbits.TON = 0; #ifdef EL_CAM_USE_STANDBY e_i2cp_enable(); // set target to register group B el_cam_register_write_uint8(0x03,0x01); // set standby bit to 1 el_cam_register_write_uint8(0x34,0x02); // set target to register group A el_cam_register_write_uint8(0x03,0x00); e_i2cp_disable(); #endif el_cam_x = 0; el_cam_y = 0; el_cam_line_pointer = (el_uint16*)el_cam_w_frame->Data; el_cam_pixel_pointer = (el_uint16*)el_cam_w_frame->Data; el_cam_enabled = 0; } }
void el_enable_camera(){ if(!el_cam_enabled){ #ifdef EL_CAM_USE_STANDBY e_i2cp_enable(); // set target to register group B el_cam_register_write_uint8(0x03,0x01); // set standby bit to 0 el_cam_register_write_uint8(0x34,0x00); // set target to register group A el_cam_register_write_uint8(0x03,0x00); e_i2cp_disable(); #endif IEC0bits.T1IE = 1;// enable pixel interrupt (T1 Interrupt) IEC1bits.T4IE = 1;// enable line interrupt (T4 Interrupt) IEC1bits.T5IE = 1;// enable frame interrupt (T5 Interrupt) T1CONbits.TON = 0; T4CONbits.TON = 0; T5CONbits.TON = 1; el_cam_enabled = 1; } }
/*!\brief Read the register reg. * \param addr The device address * \param reg The register address * \return The register value */ unsigned char readReg(unsigned char addr, unsigned char reg) { unsigned char ret; e_i2cp_enable(); ret = e_i2cp_read(addr, reg); e_i2cp_disable(); return ret; }
void getAllAxesAcc(signed int *x, signed int *y, signed int *z) { unsigned char arr[6]; e_i2cp_enable(); readRegMulti(ACC_ADDR, arr, 0x28, 6); e_i2cp_disable(); *x = (((signed int) arr[1] << 8) + (signed int) arr[0]); *y = (((signed int) arr[3] << 8) + (signed int) arr[2]); *z = (((signed int) arr[5] << 8) + (signed int) arr[4]); }
void getAllAxesGyro(signed int *x, signed int *y, signed int *z) { unsigned char arr[6]; e_i2cp_enable(); readRegMulti(GYRO_ADDR, arr, 0x28, 6); e_i2cp_disable(); *x = (((signed int) arr[1] << 8) + (signed int) arr[0]) - gyroOffsetX; *y = (((signed int) arr[3] << 8) + (signed int) arr[2]) - gyroOffsetY; *z = (((signed int) arr[5] << 8) + (signed int) arr[4]) - gyroOffsetZ; }
int getZAxisGyro() { signed char arr[2]; e_i2cp_enable(); // 2's complement readRegMulti(GYRO_ADDR, arr, 0x2C, 2); //arr[0] = readReg(GYRO_ADDR, 0x2C); //arr[1] = readReg(GYRO_ADDR, 0x2D); e_i2cp_disable(); return (((signed int) arr[1] << 8) + (signed int) arr[0]) - gyroOffsetZ; }
int getZAxisAcc() { signed char arr[2]; e_i2cp_enable(); // 2's complement readRegMulti(ACC_ADDR, arr, 0x2C, 2); //arr[0] = readReg(ACC_ADDR, 0x2C); // Z acc axis low byte //arr[1] = readReg(ACC_ADDR, 0x2D); // Z acc axis high byte e_i2cp_disable(); return ((signed int) arr[1] << 8) + (signed int) arr[0]; }
void InitComModule(unsigned char owngroup, unsigned int ownaddress, unsigned char hardwareattenuatormode, unsigned char softwareattenuatorvalue) { e_i2cp_init(); e_i2cp_enable(); while (IsModulePlugged() == 0); // wait till module online SetOwnGroup(owngroup); SetOwnAddress(ownaddress); SetHardwareAttenuator(hardwareattenuatormode); SetSoftwareAttenuator(softwareattenuatorvalue); SetRadioEnabledState(1); }
void getAllAxesGyroRaw(unsigned char *arr) { e_i2cp_enable(); readRegMulti(GYRO_ADDR, arr, 0x28, 6); /* arr[0] = readReg(GYRO_ADDR, 0x28); arr[1] = readReg(GYRO_ADDR, 0x29); arr[2] = readReg(GYRO_ADDR, 0x2A); arr[3] = readReg(GYRO_ADDR, 0x2B); arr[4] = readReg(GYRO_ADDR, 0x2C); arr[5] = readReg(GYRO_ADDR, 0x2D); */ e_i2cp_disable(); }
void getAllAxesAccRaw(unsigned char *arr) { e_i2cp_enable(); readRegMulti(ACC_ADDR, arr, 0x28, 6); /* arr[0] = readReg(ACC_ADDR, 0x28); // X acc axis low byte arr[1] = readReg(ACC_ADDR, 0x29); // X acc axis high byte arr[2] = readReg(ACC_ADDR, 0x2A); // Y acc axis low byte arr[3] = readReg(ACC_ADDR, 0x2B); // Y acc axis high byte arr[4] = readReg(ACC_ADDR, 0x2C); // Z acc axis low byte arr[5] = readReg(ACC_ADDR, 0x2D); // Z acc axis high byte */ e_i2cp_disable(); }
void e_init_randb ( unsigned char mode ) { if ( mode == I2C ) { /* Init I2C */ e_i2cp_init(); e_i2cp_enable(); } else { /* Init UART2 */ e_init_uart2(); /* Clean UART buffer */ char msg; while(e_getchar_uart2(&msg)); /* Start Agendas */ e_start_agendas_processing(); /* Locate e-randb task */ e_activate_agenda(e_randb_get_uart2, 2); /* Tell the board we work on UART mode */ e_randb_set_uart_communication(UART); } /* Calculations are made on the BOARD*/ calcOnBoard = TRUE; /* Init Global variables */ erandbFinished = FALSE; erandbState = WAITING; erandbCounter = 0; }
/*!\brief Configure and turn on the device. */ void initAccAndGyro(void) { e_i2cp_enable(); // REG5_A (0x20) = ODR (4) | BDU (1) | Axes enabling (3) // 0 1 1 0 | 1 | 1 1 1 => 0x6F => 100 Hz | BDU | x, y, z axes enabled // 0 1 1 1 | 1 | 1 1 1 => 0x7F => 400 Hz | BDU | x, y, z axes enabled // 1 0 0 0 | 1 | 1 1 1 => 0x8F => 800 Hz | BDU | x, y, z axes enabled // 1 0 0 1 | 1 | 1 1 1 => 0x9F => 1600 Hz | BDU | x, y, z axes enabled e_i2cp_write(ACC_ADDR, 0x20, 0x9F); // REG7_A (0x25) = 0 1 (enable fifo, needed?) 0 1 (auto increment address on multi read) 0 0 0 0 => 0x50 e_i2cp_write(ACC_ADDR, 0x25, 0x50); // REG6_A (0x24) => default +- 2g // FIFO_CTRL_REG_A (0x2E) => default bypass mode // CTRL_REG1_G (0x20) = ODR and cut-off (4) | mode (1) | Axes enabling (3) // 0 0 1 1 | 1 | 1 1 1 => 0x3F => odr=95Hz, cut-off=25Hz | normal | x, y, z axes enabled // 1 1 0 0 | 1 | 1 1 1 => 0xCF => odr=760Hz, cut-off=30Hz | normal | x, y, z axes enabled e_i2cp_write(GYRO_ADDR, 0x20, 0xCF); // CTRL_REG2_G (0x21) => normal mode; HPF=51.4 Hz (not used anyway) e_i2cp_write(GYRO_ADDR, 0x21, 0x20); // CTRL_REG4_G (0x23) => 250 dps (degrees per second) and continuous update e_i2cp_write(GYRO_ADDR, 0x23, 0x00); // CTRL_REG5_G (0x24) => enable fifo (needed?) //e_i2cp_write(GYRO_ADDR, 0x24, 0x40); //e_i2cp_write(GYRO_ADDR, 0x24, 0x50); // LPF1 //e_i2cp_write(GYRO_ADDR, 0x24, 0x51); // LPF1 + HPF //e_i2cp_write(GYRO_ADDR, 0x24, 0x42); // LPF1 + LPF2 //e_i2cp_write(GYRO_ADDR, 0x24, 0x52); // LPF1 + HPF + LPF2 e_i2cp_disable(); }
void el_cam_init_register(void){ e_i2cp_enable(); // read the camera model info el_cam_device_id = el_cam_register_read_uint16(0x00); el_cam_revision_n = el_cam_register_read_uint16(0x02); // set target to register group B el_cam_register_write_uint8(0x03,0x01); // change clock rate el_cam_register_write_uint8(0x38,0x08);// color format el_cam_register_write_uint8(0x68,0x01);// pixel clock rate el_cam_register_write_uint8(0x51,0x04); el_cam_register_write_uint8(0x53,0x04); el_cam_register_write_uint8(0x54,0x01); el_cam_register_write_uint8(0x55,0x43); el_cam_register_write_uint8(0x56,0x00); el_cam_register_write_uint8(0x57,0xF3); el_cam_register_write_uint8(0x61,0x0C); el_cam_register_write_uint8(0x63,0xEC); el_cam_register_write_uint8(0x80,0x40); el_cam_register_write_uint8(0x81,0x40); el_cam_register_write_uint8(0x82,0x01); // enable horizontal mirror el_cam_register_write_uint8(0x68,0b01100001); el_cam_register_write_uint8(0x03,0x00);// switch to group A el_cam_register_write_uint8(0x90,0b01110101); el_nop_delay(50); el_cam_register_write_uint8(0x03,0x01);// switch to group B el_cam_register_write_uint8(0x68,0b00000001); // use test pattern //el_cam_register_write_uint8(0x40,0x1A); // set target to register group C el_cam_register_write_uint8(0x03,0x02); el_cam_auto_function = el_cam_register_read_uint8(0x04); // change image format to 320x240 el_cam_register_write_uint8(0x11,0x12); el_cam_register_write_uint8(0x13,0x0E); el_cam_register_write_uint8(0x14,0x01); el_cam_register_write_uint8(0x15,0x30); el_cam_register_write_uint8(0x16,0x00); el_cam_register_write_uint8(0x17,0xDF); el_cam_register_write_uint8(0x19,0x72); el_cam_register_write_uint8(0x1B,0x43); el_cam_register_write_uint8(0x1D,0x50); el_cam_register_write_uint8(0x1F,0x50); // set exposure control & white balance el_cam_register_write_uint8(0x04,0b10011000);// Auto White Balance ON + AUTO Exposure Control el_cam_register_write_uint8(0x05,0b00001001);// Auto Digital Gain //el_cam_register_write_uint8(0x24,0b00000000);// External Integration Time H //el_cam_register_write_uint8(0x25,0b10000000);// External Integration Time M //el_cam_register_write_uint8(0x26,0b00000000);// External Integration Time L //el_cam_register_write_uint8(0x28,0x01);// External Linear Gain //el_cam_register_write_uint8(0x29,0x00);// External Linear Gain //el_cam_register_write_uint8(0x2C,0x00);// Exposure Time T //el_cam_register_write_uint8(0x2D,0x00);// Exposure Time H //el_cam_register_write_uint8(0x2E,0x80);// Exposure Time M //el_cam_register_write_uint8(0x2F,0x00);// Exposure Time L //el_cam_register_write_uint8(0x4C,0b00000000);// Integration Time H //el_cam_register_write_uint8(0x4D,0b10000000);// Integration Time M //el_cam_register_write_uint8(0x4E,0b00000000);// Integration Time L //el_cam_register_write_uint8(0x50,0x00);// Global Gain //el_cam_register_write_uint8(0x51,64*1.0000);// Digital Gain //el_cam_register_write_uint8(0xA4,64*1.0000);//Red Gain //el_cam_register_write_uint8(0xA5,64*1.0000);//Green Gain //el_cam_register_write_uint8(0xA6,64*1.0000);//Blue Gain #ifdef EL_CAM_USE_STANDBY // set target to register group B el_cam_register_write_uint8(0x03,0x01); el_cam_register_write_uint8(0x34,0x02);// standby bit #endif // set target to register group A el_cam_register_write_uint8(0x03,0x00); e_i2cp_disable(); }
/*!\brief Set the register reg to value * \param addr The device address * \param reg The register address * \param value The value to write */ void writeReg(unsigned char addr, unsigned char reg, unsigned char value) { e_i2cp_enable(); e_i2cp_write(addr, reg, value); e_i2cp_disable(); }
int run_asercom(void) { static char c1,c2,wait_cam=0; static int i,j,n,speedr,speedl,positionr,positionl,LED_nbr,LED_action,accx,accy,accz,sound; static int cam_mode,cam_width,cam_heigth,cam_zoom,cam_size,cam_x1,cam_y1; static char first=0; char *ptr; static int mod, reg, val; #ifdef IR_RECEIVER char ir_move = 0,ir_address= 0, ir_last_move = 0; #endif static TypeAccSpheric accelero; //static TypeAccRaw accelero_raw; int use_bt=0; //e_init_port(); // configure port pins //e_start_agendas_processing(); e_init_motors(); //e_init_uart1(); // initialize UART to 115200 Kbaud //e_init_ad_scan(); selector = getselector(); //SELECTOR0 + 2*SELECTOR1 + 4*SELECTOR2 + 8*SELECTOR3; if(selector==10) { use_bt=0; } else { use_bt=1; } #ifdef FLOOR_SENSORS if(use_bt) { // the I2C must remain disabled when using the gumstix extension e_i2cp_init(); } #endif #ifdef IR_RECEIVER e_init_remote_control(); #endif if(RCONbits.POR) { // reset if power on (some problem for few robots) RCONbits.POR=0; RESET(); } /*read HW version from the eeprom (last word)*/ static int HWversion=0xFFFF; ReadEE(0x7F,0xFFFE,&HWversion, 1); /*Cam default parameter*/ cam_mode=RGB_565_MODE; cam_width=40; // DEFAULT_WIDTH; cam_heigth=40; // DEFAULT_HEIGHT; cam_zoom=8; cam_size=cam_width*cam_heigth*2; if(use_bt) { e_poxxxx_init_cam(); //e_po6030k_set_sketch_mode(E_PO6030K_SKETCH_COLOR); e_poxxxx_config_cam((ARRAY_WIDTH -cam_width*cam_zoom)/2,(ARRAY_HEIGHT-cam_heigth*cam_zoom)/2,cam_width*cam_zoom,cam_heigth*cam_zoom,cam_zoom,cam_zoom,cam_mode); e_poxxxx_set_mirror(1,1); e_poxxxx_write_cam_registers(); } e_acc_calibr(); if(use_bt) { uart1_send_static_text("\f\a" "WELCOME to the SerCom protocol on e-Puck\r\n" "the EPFL education robot type \"H\" for help\r\n"); } else { uart2_send_static_text("\f\a" "WELCOME to the SerCom protocol on e-Puck\r\n" "the EPFL education robot type \"H\" for help\r\n"); } while(1) { if(use_bt) { while (e_getchar_uart1(&c)==0) #ifdef IR_RECEIVER { ir_move = e_get_data(); ir_address = e_get_address(); if (((ir_address == 0)||(ir_address == 8))&&(ir_move!=ir_last_move)){ switch(ir_move) { case 1: speedr = SPEED_IR; speedl = SPEED_IR/2; break; case 2: speedr = SPEED_IR; speedl = SPEED_IR; break; case 3: speedr = SPEED_IR/2; speedl = SPEED_IR; break; case 4: speedr = SPEED_IR; speedl = -SPEED_IR; break; case 5: speedr = 0; speedl = 0; break; case 6: speedr = -SPEED_IR; speedl = SPEED_IR; break; case 7: speedr = -SPEED_IR; speedl = -SPEED_IR/2; break; case 8: speedr = -SPEED_IR; speedl = -SPEED_IR; break; case 9: speedr = -SPEED_IR/2; speedl = -SPEED_IR; break; case 0: if(first==0){ e_init_sound(); first=1; } e_play_sound(11028,8016); break; default: speedr = speedl = 0; } ir_last_move = ir_move; e_set_speed_left(speedl); e_set_speed_right(speedr); } } #else ; #endif } else { while (e_getchar_uart2(&c)==0) #ifdef IR_RECEIVER { ir_move = e_get_data(); ir_address = e_get_address(); if (((ir_address == 0)||(ir_address == 8))&&(ir_move!=ir_last_move)){ switch(ir_move) { case 1: speedr = SPEED_IR; speedl = SPEED_IR/2; break; case 2: speedr = SPEED_IR; speedl = SPEED_IR; break; case 3: speedr = SPEED_IR/2; speedl = SPEED_IR; break; case 4: speedr = SPEED_IR; speedl = -SPEED_IR; break; case 5: speedr = 0; speedl = 0; break; case 6: speedr = -SPEED_IR; speedl = SPEED_IR; break; case 7: speedr = -SPEED_IR; speedl = -SPEED_IR/2; break; case 8: speedr = -SPEED_IR; speedl = -SPEED_IR; break; case 9: speedr = -SPEED_IR/2; speedl = -SPEED_IR; break; case 0: if(first==0){ e_init_sound(); first=1; } e_play_sound(11028,8016); break; default: speedr = speedl = 0; } ir_last_move = ir_move; e_set_speed_left(speedl); e_set_speed_right(speedr); } } #else ; #endif } if (c<0) { // binary mode (big endian) i=0; do { switch(-c) { case 'a': // Read acceleration sensors in a non // filtered way, some as ASCII accx = e_get_acc_filtered(0, 1); accy = e_get_acc_filtered(1, 1); accz = e_get_acc_filtered(2, 1); //accx = e_get_acc(0); //too much noisy //accy = e_get_acc(1); //accz = e_get_acc(2); buffer[i++] = accx & 0xff; buffer[i++] = accx >> 8; buffer[i++] = accy & 0xff; buffer[i++] = accy >> 8; buffer[i++] = accz & 0xff; buffer[i++] = accz >> 8; /* accelero_raw=e_read_acc_xyz(); ptr=(char *)&accelero_raw.acc_x; buffer[i++]=(*ptr); ptr++; buffer[i++]=(*ptr); ptr++; ptr=(char *)&accelero_raw.acc_y; buffer[i++]=(*ptr); ptr++; buffer[i++]=(*ptr); ptr++; ptr=(char *)&accelero_raw.acc_z; buffer[i++]=(*ptr); ptr++; buffer[i++]=(*ptr); ptr++; */ break; case 'A': // read acceleration sensors accelero=e_read_acc_spheric(); ptr=(char *)&accelero.acceleration; buffer[i++]=(*ptr); ptr++; buffer[i++]=(*ptr); ptr++; buffer[i++]=(*ptr); ptr++; buffer[i++]=(*ptr); ptr=(char *)&accelero.orientation; buffer[i++]=(*ptr); ptr++; buffer[i++]=(*ptr); ptr++; buffer[i++]=(*ptr); ptr++; buffer[i++]=(*ptr); ptr=(char *)&accelero.inclination; buffer[i++]=(*ptr); ptr++; buffer[i++]=(*ptr); ptr++; buffer[i++]=(*ptr); ptr++; buffer[i++]=(*ptr); break; case 'b': // battery ok? buffer[i++] = BATT_LOW; break; case 'D': // set motor speed if(use_bt) { while (e_getchar_uart1(&c1)==0); while (e_getchar_uart1(&c2)==0); } else { while (e_getchar_uart2(&c1)==0); while (e_getchar_uart2(&c2)==0); } speedl=(unsigned char)c1+((unsigned int)c2<<8); if(use_bt) { while (e_getchar_uart1(&c1)==0); while (e_getchar_uart1(&c2)==0); } else { while (e_getchar_uart2(&c1)==0); while (e_getchar_uart2(&c2)==0); } speedr=(unsigned char)c1+((unsigned int)c2<<8); e_set_speed_left(speedl); e_set_speed_right(speedr); break; case 'E': // get motor speed buffer[i++] = speedl & 0xff; buffer[i++] = speedl >> 8; buffer[i++] = speedr & 0xff; buffer[i++] = speedr >> 8; break; case 'I': // get camera image if(use_bt) { e_poxxxx_launch_capture(&buffer[i+3]); wait_cam=1; buffer[i++]=(char)cam_mode&0xff;//send image parameter buffer[i++]=(char)cam_width&0xff; buffer[i++]=(char)cam_heigth&0xff; i+=cam_size; } break; case 'L': // set LED if(use_bt) { while (e_getchar_uart1(&c1)==0); while (e_getchar_uart1(&c2)==0); } else { while (e_getchar_uart2(&c1)==0); while (e_getchar_uart2(&c2)==0); } switch(c1) { case 8: if(use_bt) { e_set_body_led(c2); } break; case 9: if(use_bt) { e_set_front_led(c2); } break; default: e_set_led(c1,c2); break; } break; case 'M': // optional floor sensors #ifdef FLOOR_SENSORS if(use_bt) { e_i2cp_init(); e_i2cp_enable(); e_i2cp_read(0xC0, 0); for(j = 0; j < 6; j++) { if (j % 2 == 0) buffer[i++] = e_i2cp_read(0xC0, j + 1); else buffer[i++] = e_i2cp_read(0xC0, j - 1); } #ifdef CLIFF_SENSORS for(j=13; j<17; j++) { if (j % 2 == 0) buffer[i++] = e_i2cp_read(0xC0, j - 1); else buffer[i++] = e_i2cp_read(0xC0, j + 1); } #endif e_i2cp_disable(); } #else for(j=0;j<6;j++) buffer[i++]=0; #endif break; case 'N': // read proximity sensors if(use_bt) { for(j=0;j<8;j++) { n=e_get_calibrated_prox(j); // or ? n=e_get_prox(j); buffer[i++]=n&0xff; buffer[i++]=n>>8; } } else { for(j=0;j<10;j++) { n=e_get_calibrated_prox(j); // or ? n=e_get_prox(j); buffer[i++]=n&0xff; buffer[i++]=n>>8; } } break; case 'O': // read light sensors if(use_bt) { for(j=0;j<8;j++) { n=e_get_ambient_light(j); buffer[i++]=n&0xff; buffer[i++]=n>>8; } } else { for(j=0;j<10;j++) { n=e_get_ambient_light(j); buffer[i++]=n&0xff; buffer[i++]=n>>8; } } break; case 'Q': // read encoders n=e_get_steps_left(); buffer[i++]=n&0xff; buffer[i++]=n>>8; n=e_get_steps_right(); buffer[i++]=n&0xff; buffer[i++]=n>>8; break; case 'u': // get last micro volumes n = e_get_micro_volume(0); buffer[i++] = n & 0xff; buffer[i++] = n >> 8; n = e_get_micro_volume(1); buffer[i++] = n & 0xff; buffer[i++] = n >> 8; n = e_get_micro_volume(2); buffer[i++] = n & 0xff; buffer[i++] = n >> 8; break; case 'U': // get micro buffer ptr=(char *)e_mic_scan; if(use_bt) { e_send_uart1_char(ptr,600);//send sound buffer } else { e_send_uart2_char(ptr,600);//send sound buffer } n=e_last_mic_scan_id;//send last scan buffer[i++]=n&0xff; break; default: // silently ignored break; } if(use_bt) { while (e_getchar_uart1(&c)==0); // get next command } else { while (e_getchar_uart2(&c)==0); // get next command } } while(c);
int main(void) { // Locals General. ----------------------------------------------------- char imu_board_id[9] = {0,0,0,0, 0,0,0,0, 0}; char message[1024]; // Any message to send by UART1 InitOscillator(); // Initialize the PLL (also disables wdt) __delay32(_50MILLISEC); // Init mcu ports ------------------------------------------------------ init_port(); // Initialize ports LED_ORNG =0; LED_RED = 1; // Init UARTS. --------------------------------------------------------- init_UART1(); // Initialize the serial communication (TTL / RS-232) init_UART2(); broadcast_message("RC Testing program\n"); // Init Analog Channels. ----------------------------------------------- analog_initAnalog(); // Init the ADC module // Init SPI. --------------------------------------------------------- init_SPI(); // Init I2C. --------------------------------------------------------- e_i2cp_init(); e_i2cp_enable(); __delay32(_50MILLISEC); e_i2c_write(0x00); // dummy byte to get the START bit on the bus (believe it or not!) // Init RC. ---------------------------------------------------------- initControlVariables(NULL); broadcast_message("Initialising RC\n"); LED_ORNG =1; LED_RED = 1; BuzzerBip(1,1); // Make x bips of the Buzzer (blocking) RCInitReception(); // RCSetType(RC_WK2401); RCSetType(RC_WK2402); broadcast_message("Initialising IMU\n"); // Init BUZZER. ---------------------------------------------------------- __delay32(_200MILLISEC); // Wait for the IMU to boot while (INT_IMU==0) // Wait for the IMU to boot { FlashORNG (); // Flash the LED } read_imu_version(imu_board_id); imu_board_id[8] = 0; broadcast_message("Entering main loop\n"); // Init BUZZER. ---------------------------------------------------------- BuzzerBip(3,1); // Make x bips of the Buzzer, blocking InitLoopTimer(control.params.T_ctrl_ms);// Initialize & Enable control loop timer (20ms; 50Hz) LED_RED = 0; while (1) { __delay32(5*_10MILLISEC); sprintf(message, "RC %+.2f %+.2f %+.2f %.2f %.2f %+.2f %+.2f %+.2f %s CTRL %02X %s %s\n", RC_THROTTLE, RC_YAW, RC_ROLL, RC_PITCH, RC_THROTTLE_TRIM, RC_YAW_TRIM, RC_ROLL_TRIM, RC_PITCH_TRIM, string_of_rc_state(RCSMGetState()), control.flags.CONTROL_MODE, string_of_control_mode(control.flags.CONTROL_MODE), string_of_control_type(control.flags.CONTROL_MODE)); broadcast_message(message); } return 0; } // End of main
void el_config_camera(const el_camera_param*p){ union{ uint32_t dword; struct{ uint8_t byteL; uint8_t byteM; uint8_t byteH; uint8_t byteT; }; } FixedPoint; union{ uint8_t byte; struct{ uint8_t ExposureMode:2; uint8_t AutoWhiteBalanceOFF:1; }; } AutoControl_1; union{ uint8_t byte; struct{ uint8_t Reserved:2; uint8_t AutoDigitalGain:1; }; } AutoControl_2; AutoControl_1.byte = 0b10011000; AutoControl_2.byte = 0b00001101; AutoControl_1.ExposureMode = p->ExposureMode; AutoControl_1.AutoWhiteBalanceOFF = !p->AutoWhiteBalance; AutoControl_2.AutoDigitalGain = p->AutoDigitalGain; e_i2cp_enable(); el_cam_register_write_uint8(0x03,0x02);// Register Group C el_cam_register_write_uint8(0x04,AutoControl_1.byte); el_cam_register_write_uint8(0x05,AutoControl_2.byte); FixedPoint.dword = 32768*p->ExternalIntergationTime; el_cam_register_write_uint8(0x24,FixedPoint.byteH);// External Integration Time H el_cam_register_write_uint8(0x25,FixedPoint.byteM);// External Integration Time M el_cam_register_write_uint8(0x26,FixedPoint.byteL);// External Integration Time L FixedPoint.dword = 256*p->ExternalLinearGain; el_cam_register_write_uint8(0x28,FixedPoint.byteM);// External Linear Gain H el_cam_register_write_uint8(0x29,FixedPoint.byteL);// External Linear Gain L FixedPoint.dword = 32768*p->ExposureTime; el_cam_register_write_uint8(0x2C,FixedPoint.byteT);// Exposure Time T el_cam_register_write_uint8(0x2D,FixedPoint.byteH);// Exposure Time H el_cam_register_write_uint8(0x2E,FixedPoint.byteM);// Exposure Time M el_cam_register_write_uint8(0x2F,FixedPoint.byteL);// Exposure Time L FixedPoint.dword = 32768*p->IntegrationTime; el_cam_register_write_uint8(0x4C,FixedPoint.byteH);// Integration Time H el_cam_register_write_uint8(0x4D,FixedPoint.byteM);// Integration Time M el_cam_register_write_uint8(0x4E,FixedPoint.byteL);// Integration Time L FixedPoint.dword = (int)(15.8f*log(p->GlobalGain)/log(2.0f)); el_cam_register_write_uint8(0x50,FixedPoint.byteL);// Global Gain el_cam_register_write_uint8(0x51,64*p->DigitalGain); el_cam_register_write_uint8(0xA4,64*p->RedGain); el_cam_register_write_uint8(0xA5,64*p->GreenGain); el_cam_register_write_uint8(0xA6,64*p->BlueGain); el_cam_register_write_uint8(0x03,0x00);// Register Group A e_i2cp_disable(); }