/** * Initializing the cyrf chip */ void cyrf6936_init(struct Cyrf6936 *cyrf, struct spi_periph *spi_p, const uint8_t slave_idx, const uint32_t rst_port, const uint16_t rst_pin) { /* Set spi_peripheral and the status */ cyrf->spi_p = spi_p; cyrf->status = CYRF6936_UNINIT; cyrf->has_irq = FALSE; /* Set the spi transaction */ cyrf->spi_t.cpol = SPICpolIdleLow; cyrf->spi_t.cpha = SPICphaEdge1; cyrf->spi_t.dss = SPIDss8bit; cyrf->spi_t.bitorder = SPIMSBFirst; cyrf->spi_t.cdiv = SPIDiv64; cyrf->spi_t.input_length = 0; cyrf->spi_t.output_length = 0; cyrf->spi_t.input_buf = cyrf->input_buf; cyrf->spi_t.output_buf = cyrf->output_buf; cyrf->spi_t.slave_idx = slave_idx; cyrf->spi_t.select = SPISelectUnselect; cyrf->spi_t.status = SPITransDone; /* Reset the CYRF6936 chip (busy waiting) */ gpio_setup_output(rst_port, rst_pin); gpio_set(rst_port, rst_pin); sys_time_usleep(100); gpio_clear(rst_port, rst_pin); sys_time_usleep(100); /* Get the MFG ID */ cyrf->status = CYRF6936_GET_MFG_ID; cyrf->buffer_idx = 0; cyrf6936_write_register(cyrf, CYRF_MFG_ID, 0xFF); }
int main(void) { main_init(); #if LIMIT_EVENT_POLLING /* Limit main loop frequency to 1kHz. * This is a kludge until we can better leverage threads and have real events. * Without this limit the event flags will constantly polled as fast as possible, * resulting on 100% cpu load on boards with an (RT)OS. * On bare metal boards this is not an issue, as you have nothing else running anyway. */ uint32_t t_begin = 0; uint32_t t_diff = 0; while (1) { t_begin = get_sys_time_usec(); handle_periodic_tasks(); main_event(); /* sleep remaining time to limit to 1kHz */ t_diff = get_sys_time_usec() - t_begin; if (t_diff < 1000) { sys_time_usleep(1000 - t_diff); } } #else while (1) { handle_periodic_tasks(); main_event(); } #endif return 0; }
int main(void) { mcu_init(); /* * Init threads */ chThdCreateStatic(wa_thd_main_periodic_05, sizeof(wa_thd_main_periodic_05), NORMALPRIO, thd_main_periodic_05, NULL); while (1) { /* sleep for 1s */ sys_time_ssleep(1); main_periodic_1(); /* sleep for 0.42s */ /* * sys_time_usleep(uint32_t us) * Use only for up to 2^32/CH_CFG_ST_FREQUENCY-1 usec * e.g. if CH_CFG_ST_FREQUENCY=10000 use max for 420000 us * or 420ms, otherwise overflow happens */ sys_time_usleep(420000); main_periodic_15(); } return 0; }
void pcap01_init(void) { pcap01_trans.status = I2CTransDone; pcap01Value.status = PCAP01_IDLE; #ifdef PCAP01_LOAD_FIRMWARE writePCAP01_firmware(); #endif pcap01writeRegister(PCAP01_REG0, PCAP01_REG0_VALUE); pcap01writeRegister(PCAP01_REG1, PCAP01_REG1_VALUE); pcap01writeRegister(PCAP01_REG2, PCAP01_REG2_VALUE); pcap01writeRegister(PCAP01_REG3, PCAP01_REG3_VALUE); pcap01writeRegister(PCAP01_REG4, PCAP01_REG4_VALUE); pcap01writeRegister(PCAP01_REG5, PCAP01_REG5_VALUE); pcap01writeRegister(PCAP01_REG6, PCAP01_REG6_VALUE); pcap01writeRegister(PCAP01_REG7, PCAP01_REG7_VALUE); pcap01writeRegister(PCAP01_REG8, PCAP01_REG8_VALUE); pcap01writeRegister(PCAP01_REG9, PCAP01_REG9_VALUE); pcap01writeRegister(PCAP01_REG10, PCAP01_REG10_VALUE); pcap01writeRegister(PCAP01_REG11, PCAP01_REG11_VALUE); pcap01writeRegister(PCAP01_REG12, PCAP01_REG12_VALUE); pcap01writeRegister(PCAP01_REG13, PCAP01_REG13_VALUE); pcap01writeRegister(PCAP01_REG14, PCAP01_REG14_VALUE); pcap01writeRegister(PCAP01_REG15, PCAP01_REG15_VALUE); pcap01writeRegister(PCAP01_REG16, PCAP01_REG16_VALUE); pcap01writeRegister(PCAP01_REG17, PCAP01_REG17_VALUE); pcap01writeRegister(PCAP01_REG18, PCAP01_REG18_VALUE); pcap01writeRegister(PCAP01_REG19, PCAP01_REG19_VALUE); pcap01writeRegister(PCAP01_REG20, PCAP01_REG20_VALUE); PCAP01_Control(PCAP01_IN_RESET); sys_time_usleep(500000); PCAP01_Control(PCAP01_START); }
int main(void) { mcu_init(); sys_time_register_timer(0.5, main_periodic_05); mcu_int_enable(); while (1) { /* sleep for 1s */ sys_time_usleep(1000000); main_periodic_1(); /* sleep for 0.5s */ sys_time_usleep(500000); main_periodic_15(); } return 0; }
void writePCAP01_firmware(void) { int i = 0; char testbyte = 44; uint16_t testaddress = 71; // Hard reset PCAP01_Control(PCAP01_PU_RESET); sys_time_usleep(50000); //write testbyte writePCAP01_SRAM(testbyte, testaddress); //check testbyte if (readPCAP01_SRAM(testaddress) != testbyte) { return; } else { LED_ON(3); //Hard reset PCAP01_Control(PCAP01_PU_RESET); //write firmware for (i = 0; i < sizeof(firmware); i++) { writePCAP01_SRAM(firmware[i], i); } //fill with ffs for (; i < 4029; i++) { writePCAP01_SRAM(0xff, i); } i++; #ifdef PCAP01_200HZ //write end bytes of sram writePCAP01_SRAM(0x04, i++); writePCAP01_SRAM(0x01, i++); writePCAP01_SRAM(0x01, i++); #endif #ifdef PCAP01_STANDARD //write end bytes of sram writePCAP01_SRAM(0x02, i++); writePCAP01_SRAM(0x01, i++); writePCAP01_SRAM(0x03, i++); #endif } }
void init_ap( void ) { #ifndef SINGLE_MCU /** init done in main_fbw in single MCU */ mcu_init(); sys_time_init(); #endif /* SINGLE_MCU */ /************* Sensors initialization ***************/ #ifdef USE_INFRARED infrared_init(); #endif #ifdef USE_GYRO gyro_init(); #endif #ifdef USE_GPS gps_init(); #endif #ifdef USE_GPIO GpioInit(); #endif #ifdef USE_IMU imu_init(); #endif #ifdef USE_AHRS ahrs_aligner_init(); ahrs_init(); #endif /************* Links initialization ***************/ #if defined MCU_SPI_LINK link_mcu_init(); #endif #ifdef MODEM modem_init(); #endif /************ Internal status ***************/ h_ctl_init(); v_ctl_init(); estimator_init(); #ifdef ALT_KALMAN alt_kalman_init(); #endif nav_init(); modules_init(); settings_init(); /** - start interrupt task */ mcu_int_enable(); /** wait 0.5s (historical :-) */ sys_time_usleep(500000); #if defined GPS_CONFIGURE gps_configure_uart(); #endif #if defined DATALINK #if DATALINK == XBEE xbee_init(); #endif #endif /* DATALINK */ #if defined AEROCOMM_DATA_PIN IO0DIR |= _BV(AEROCOMM_DATA_PIN); IO0SET = _BV(AEROCOMM_DATA_PIN); #endif power_switch = FALSE; /************ Multi-uavs status ***************/ #ifdef TRAFFIC_INFO traffic_info_init(); #endif }
void w5100_init( void ) { // configure the SPI bus. w5100_spi.slave_idx = W5100_SLAVE_IDX; w5100_spi.output_length = 4; w5100_spi.input_length = 4; w5100_spi.select = SPISelectUnselect; w5100_spi.cpol = SPICpolIdleLow; w5100_spi.cpha = SPICphaEdge1; w5100_spi.dss = SPIDss8bit; w5100_spi.bitorder = SPIMSBFirst; w5100_spi.cdiv = SPIDiv64; chip0.status = W5100StatusUninit; chip0.curbuf = 0; w5100_spi.input_buf = &chip0.work_rx[0]; w5100_spi.output_buf = &chip0.work_tx[0]; // wait one second for proper initialization (chip getting powered up). sys_time_usleep(1000000); // set DRDY pin gpio_setup_output(W5100_DRDY_GPIO, W5100_DRDY_GPIO_PIN); gpio_clear(W5100_DRDY_GPIO, W5100_DRDY_GPIO_PIN); sys_time_usleep(200); gpio_set(W5100_DRDY_GPIO, W5100_DRDY_GPIO_PIN); // allow some time for the chip to wake up. sys_time_usleep(20000); // write reset bit into mode register w5100_set( REG_MR, 1<<RST ); // allow some time to wake up... sys_time_usleep(20000); // receive memory size w5100_set( REG_RX_MEM, 0x55 ); // transmit memory size w5100_set( REG_TX_MEM, 0x55 ); // Setting the required socket base addresses for reads and writes to/from sockets for (int i=0; i<SOCKETS; i++) { SBASE[i] = TXBUF_BASE + SSIZE * i; RBASE[i] = RXBUF_BASE + RSIZE * i; } uint8_t gateway[4]; gateway[0] = ip[0]; gateway[1] = ip[1]; gateway[2] = ip[2]; gateway[3] = 1; // configure gateway, subnet, mac and ip on "NIC". w5100_set_buffer( REG_GAR, gateway, 4 ); w5100_set_buffer( REG_SUBR, subnet, 4 ); w5100_set_buffer( REG_SHAR, mac, 6 ); w5100_set_buffer( REG_SIPR, ip, 4 ); // create a socket to send telemetry through. configure_socket( TELEM_SOCKET, SNMR_MULTI, 1, dport, dest ); // make dest zero and configure socket to receive data dest[ 0 ] = 0x00; configure_socket( CMD_SOCKET, 0, dport, dport, dest ); }
void init_ap( void ) { #ifndef SINGLE_MCU /** init done in main_fbw in single MCU */ mcu_init(); #endif /* SINGLE_MCU */ /************* Sensors initialization ***************/ #if USE_GPS gps_init(); #endif #ifdef USE_GPIO GpioInit(); #endif #if USE_IMU imu_init(); #endif #if USE_AHRS_ALIGNER ahrs_aligner_init(); #endif #if USE_AHRS ahrs_init(); #endif #if USE_BAROMETER baro_init(); #endif ins_init(); stateInit(); /************* Links initialization ***************/ #if defined MCU_SPI_LINK || defined MCU_UART_LINK link_mcu_init(); #endif #if USE_AUDIO_TELEMETRY audio_telemetry_init(); #endif /************ Internal status ***************/ autopilot_init(); h_ctl_init(); v_ctl_init(); nav_init(); modules_init(); settings_init(); /**** start timers for periodic functions *****/ sensors_tid = sys_time_register_timer(1./PERIODIC_FREQUENCY, NULL); navigation_tid = sys_time_register_timer(1./NAVIGATION_FREQUENCY, NULL); attitude_tid = sys_time_register_timer(1./CONTROL_FREQUENCY, NULL); modules_tid = sys_time_register_timer(1./MODULES_FREQUENCY, NULL); telemetry_tid = sys_time_register_timer(1./60, NULL); monitor_tid = sys_time_register_timer(1.0, NULL); /** - start interrupt task */ mcu_int_enable(); #if defined DATALINK #if DATALINK == XBEE xbee_init(); #endif #if DATALINK == W5100 w5100_init(); #endif #endif /* DATALINK */ #if defined AEROCOMM_DATA_PIN IO0DIR |= _BV(AEROCOMM_DATA_PIN); IO0SET = _BV(AEROCOMM_DATA_PIN); #endif /************ Multi-uavs status ***************/ #ifdef TRAFFIC_INFO traffic_info_init(); #endif #ifdef SET_IMU_ZERO_ON_STARTUP #ifndef SITL //wait 10secs for init sys_time_usleep(10000000); imu_store_bias(); #endif #endif }
void init_ap( void ) { #ifndef SINGLE_MCU /** init done in main_fbw in single MCU */ hw_init(); sys_time_init(); #ifdef LED led_init(); #endif #ifdef ADC adc_init(); #endif #endif /* SINGLE_MCU */ /************* Sensors initialization ***************/ #ifdef USE_INFRARED ir_init(); #endif #ifdef USE_GYRO gyro_init(); #endif #ifdef USE_GPS gps_init(); #endif #ifdef USE_UART0 Uart0Init(); #endif #ifdef USE_UART1 Uart1Init(); #endif #ifdef USE_UART2 Uart2Init(); #endif #ifdef USE_UART3 Uart3Init(); #endif #ifdef USE_USB_SERIAL VCOM_init(); #endif #ifdef USE_GPIO GpioInit(); #endif #ifdef USE_I2C0 i2c0_init(); #endif #ifdef USE_I2C1 i2c1_init(); #endif #ifdef USE_I2C2 i2c2_init(); #endif /************* Links initialization ***************/ #if defined USE_SPI spi_init(); #endif #if defined MCU_SPI_LINK link_mcu_init(); #endif #ifdef MODEM modem_init(); #endif /************ Internal status ***************/ h_ctl_init(); v_ctl_init(); estimator_init(); #ifdef ALT_KALMAN alt_kalman_init(); #endif nav_init(); modules_init(); /** - start interrupt task */ int_enable(); /** wait 0.5s (historical :-) */ sys_time_usleep(500000); #if defined GPS_CONFIGURE gps_configure_uart(); #endif #if defined DATALINK #if DATALINK == XBEE xbee_init(); #endif #endif /* DATALINK */ #if defined AEROCOMM_DATA_PIN IO0DIR |= _BV(AEROCOMM_DATA_PIN); IO0SET = _BV(AEROCOMM_DATA_PIN); #endif power_switch = FALSE; /************ Multi-uavs status ***************/ #ifdef TRAFFIC_INFO traffic_info_init(); #endif }
void px4flash_event(void) { if (PX4IO_PORT->char_available(PX4IO_PORT->periph)) { if (!setToBootloaderMode) { //ignore anything coming from IO if not in bootloader mode (which should be nothing) } else { //relay everything from IO to the laptop while (PX4IO_PORT->char_available(PX4IO_PORT->periph)) { unsigned char b = PX4IO_PORT->get_byte(PX4IO_PORT->periph); FLASH_PORT->put_byte(FLASH_PORT->periph, b); } } } //TODO: check if bootloader timeout was surpassed if (FLASH_PORT->char_available(FLASH_PORT->periph) && !setToBootloaderMode) { // TMP TEST // while (FLASH_PORT->char_available(FLASH_PORT->periph)) { // unsigned char bla = FLASH_PORT->get_byte(FLASH_PORT->periph); // FLASH_PORT->put_byte(FLASH_PORT->periph,bla); // } // return; //check whether this is flash related communication, and for who (ap/fbw) int state = 0; while (state < 4 && FLASH_PORT->char_available(FLASH_PORT->periph)) { unsigned char b = FLASH_PORT->get_byte(FLASH_PORT->periph); switch (state) { case (0) : if (b == 'p') { state++; } else { return; } break; case (1) : if (b == 'p') { state++; } else { return; } break; case (2) : if (b == 'r') { state++; } else { return; } break; case (3) : if (b == 'z') { state++; } else { return; } break; default : break; } } if (state != 4) {return;} //TODO: check if/how this interferes with flashing original PX4 firmware unsigned char target = FLASH_PORT->get_byte(FLASH_PORT->periph); if (target == '1') { //target ap //the target is the ap, so reboot to PX4 bootloader scb_reset_system(); } else { // target fbw //the target is the fbw, so reboot the fbw and switch to relay mode //first check if the bootloader has not timeout: if (sys_time_check_and_ack_timer(px4iobl_tid) || px4ioRebootTimeout) { px4ioRebootTimeout = TRUE; sys_time_cancel_timer(px4iobl_tid); FLASH_PORT->put_byte(FLASH_PORT->periph, 'T'); FLASH_PORT->put_byte(FLASH_PORT->periph, 'I'); FLASH_PORT->put_byte(FLASH_PORT->periph, 'M'); FLASH_PORT->put_byte(FLASH_PORT->periph, 'E'); FLASH_PORT->put_byte(FLASH_PORT->periph, 'O'); FLASH_PORT->put_byte(FLASH_PORT->periph, 'U'); FLASH_PORT->put_byte(FLASH_PORT->periph, 'T'); // use 7 chars as answer return; } { // FBW OK OK hollay hollay :) FLASH_PORT->put_byte(FLASH_PORT->periph, 'F'); FLASH_PORT->put_byte(FLASH_PORT->periph, 'B'); FLASH_PORT->put_byte(FLASH_PORT->periph, 'W'); FLASH_PORT->put_byte(FLASH_PORT->periph, 'O'); FLASH_PORT->put_byte(FLASH_PORT->periph, 'K'); FLASH_PORT->put_byte(FLASH_PORT->periph, 'O'); FLASH_PORT->put_byte(FLASH_PORT->periph, 'K'); // use 7 chars as answer } //stop all intermcu communication: disable_inter_comm(true); /* * The progdieshit define is very usefull, if for whatever reason the (normal, not bootloader) firmware on the IO chip became disfunct. * In that case: * 1. enable this define * 2. build and upload the fmu f4 chip (ap target in pprz center) * 3. build the io code, and convert the firmware using the following command: * /home/houjebek/paparazzi/sw/tools/px4/px_mkfw.py --prototype "/home/houjebek/px4/Firmware/Images/px4io-v2.prototype" --image /home/houjebek/paparazzi/var/aircrafts/Iris/fbw/fbw.bin > /home/houjebek/paparazzi/var/aircrafts/Iris/fbw/fbw.px4 * 4. Start the following command: * /home/houjebek/paparazzi/sw/tools/px4/px_uploader.py --port "/dev/ttyACM0" /home/houjebek/paparazzi/var/aircrafts/Iris/fbw/fbw.px4 * 5a. Either, boot the Pixhawk (reconnect usb) holding the IO reset button until the FMU led stops blinking fast (i.e. exits its own bootloader) * 5b Or, press the IO reset button on the pixhawk * 6. Watch the output of the command of step 4, it should recognize the IO bootloader and start flashing. If not try repeating step 5a. * 7. Don forget to disable the define and upload the ap again :) */ // #define progdieshit #ifndef progdieshit //send the reboot to bootloader command: static struct IOPacket dma_packet; dma_packet.count_code = 0x40 + 0x01; dma_packet.crc = 0; dma_packet.page = PX4IO_PAGE_SETUP; dma_packet.offset = PX4IO_P_SETUP_REBOOT_BL; dma_packet.regs[0] = PX4IO_REBOOT_BL_MAGIC; dma_packet.crc = crc_packet(&dma_packet); struct IOPacket *pkt = &dma_packet; uint8_t *p = (uint8_t *)pkt; PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[0]); PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[1]); PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[2]); PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[3]); PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[4]); PX4IO_PORT->put_byte(PX4IO_PORT->periph, p[5]); sys_time_usleep(5000); // this seems to be close to the minimum delay necessary to process this packet at the IO side //the pixhawk IO chip should respond with: // 0x00 ( PKT_CODE_SUCCESS ) // 0xe5 // 0x32 // 0x0a //After that, the IO chips reboots into bootloader mode, in which it will stay for a short period //The baudrate in bootloader mode ic changed to 115200 (normal operating baud is 1500000, at least for original pixhawk fmu firmware) //state machine state = 0; while (state < 4 && PX4IO_PORT->char_available(PX4IO_PORT->periph)) { unsigned char b = PX4IO_PORT->get_byte(PX4IO_PORT->periph); switch (state) { case (0) : if (b == PKT_CODE_SUCCESS) { state++; } else { state = 0; } break; case (1) : if (b == 0xe5) { state++; } else { state = 0; } break; case (2) : if (b == 0x32) { state++; } else { state = 0; } break; case (3) : if (b == 0x0a) { state++; } else { state = 0; } break; default : break; } } #else int state = 4; #endif if (state == 4) { uart_periph_set_baudrate(PX4IO_PORT->periph, B115200); /* look for the bootloader for 150 ms */ int ret = 0; for (int i = 0; i < 15 && !ret ; i++) { sys_time_usleep(10000); //send a get_sync command in order to keep the io in bootloader mode PX4IO_PORT->put_byte(PX4IO_PORT->periph, PROTO_GET_SYNC); PX4IO_PORT->put_byte(PX4IO_PORT->periph, PROTO_EOC); //get_sync should be replied with, so check if that happens and //all other bytes are discarded, hopefully those were not important //(they may be caused by sending multiple syncs) while (PX4IO_PORT->char_available(PX4IO_PORT->periph)) { unsigned char b = PX4IO_PORT->get_byte(PX4IO_PORT->periph); if (b == PROTO_INSYNC) { setToBootloaderMode = true; ret = 1; break; } } } if (setToBootloaderMode) { //if successfully entered bootloader mode, clear any remaining bytes (which may have a function, but I did not check) while (PX4IO_PORT->char_available(PX4IO_PORT->periph)) {PX4IO_PORT->get_byte(PX4IO_PORT->periph);} } } else { FLASH_PORT->put_byte(FLASH_PORT->periph, 'E'); //TODO: find out what the PX4 protocol for error feedback is... FLASH_PORT->put_byte(FLASH_PORT->periph, 'R'); FLASH_PORT->put_byte(FLASH_PORT->periph, 'R'); FLASH_PORT->put_byte(FLASH_PORT->periph, 'O'); FLASH_PORT->put_byte(FLASH_PORT->periph, 'R'); FLASH_PORT->put_byte(FLASH_PORT->periph, '!'); FLASH_PORT->put_byte(FLASH_PORT->periph, ' '); // use 7 chars as answer } } } else if (FLASH_PORT->char_available(FLASH_PORT->periph)) { //already in bootloader mode, just directly relay data while (FLASH_PORT->char_available(FLASH_PORT->periph)) { unsigned char b = FLASH_PORT->get_byte(FLASH_PORT->periph); PX4IO_PORT->put_byte(PX4IO_PORT->periph, b); } } }