static void rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) { uint16_t sr = rSR; stm32_dmastop(rx_dma); stm32_dmastop(tx_dma); /* handle the received packet */ rx_handle_packet(); /* re-set DMA for reception first, so we are ready to receive before we start sending */ if (!(sr & SPI_SR_BSY)) { dma_reset(); } /* send the reply to the just-processed request */ dma_packet.crc = 0; dma_packet.crc = crc_packet(&dma_packet); stm32_dmasetup( tx_dma, (uint32_t)&rDR, (uint32_t)&dma_packet, PKT_SIZE(dma_packet), DMA_CCR_DIR | DMA_CCR_MINC | DMA_CCR_PSIZE_8BITS | DMA_CCR_MSIZE_8BITS | DMA_CCR_PRIMED); stm32_dmastart(tx_dma, NULL, NULL, false); perf_end(pc_txns); }
static void rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) { /* * We are here because DMA completed, or UART reception stopped and * we think we have a packet in the buffer. */ perf_begin(pc_txns); /* disable UART DMA */ rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); /* handle the received packet */ rx_handle_packet(); /* re-set DMA for reception first, so we are ready to receive before we start sending */ dma_reset(); /* send the reply to the just-processed request */ dma_packet.crc = 0; dma_packet.crc = crc_packet(&dma_packet); stm32_dmasetup( tx_dma, (uint32_t)&rDR, (uint32_t)&dma_packet, PKT_SIZE(dma_packet), DMA_CCR_DIR | DMA_CCR_MINC | DMA_CCR_PSIZE_8BITS | DMA_CCR_MSIZE_8BITS); stm32_dmastart(tx_dma, NULL, NULL, false); rCR3 |= USART_CR3_DMAT; perf_end(pc_txns); }
static void set_hf_send_packet(int fd_out, int fd_in, unsigned char buffer[], unsigned char len, unsigned char *seq, unsigned char type, unsigned char response[], int *response_length) { set_packet_header(buffer, len, *seq, type); crc_packet(tabel, buffer, len); send_packet(fd_out, fd_in, buffer, len + 2, response, response_length); increment_counter(seq); }
static void rx_handle_packet(void) { /* check packet CRC */ uint8_t crc = dma_packet.crc; dma_packet.crc = 0; if (crc != crc_packet(&dma_packet)) { perf_count(pc_crcerr); /* send a CRC error reply */ dma_packet.count_code = PKT_CODE_CORRUPT; dma_packet.page = 0xff; dma_packet.offset = 0xff; return; } if (PKT_CODE(dma_packet) == PKT_CODE_WRITE) { /* it's a blind write - pass it on */ if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], PKT_COUNT(dma_packet))) { perf_count(pc_regerr); dma_packet.count_code = PKT_CODE_ERROR; } else { dma_packet.count_code = PKT_CODE_SUCCESS; } return; } if (PKT_CODE(dma_packet) == PKT_CODE_READ) { /* it's a read - get register pointer for reply */ unsigned count; uint16_t *registers; if (registers_get(dma_packet.page, dma_packet.offset, ®isters, &count) < 0) { perf_count(pc_regerr); dma_packet.count_code = PKT_CODE_ERROR; } else { /* constrain reply to requested size */ if (count > PKT_MAX_REGS) count = PKT_MAX_REGS; if (count > PKT_COUNT(dma_packet)) count = PKT_COUNT(dma_packet); /* copy reply registers into DMA buffer */ memcpy((void *)&dma_packet.regs[0], registers, count * 2); dma_packet.count_code = count | PKT_CODE_SUCCESS; } return; } /* send a bad-packet error reply */ dma_packet.count_code = PKT_CODE_CORRUPT; dma_packet.page = 0xff; dma_packet.offset = 0xfe; }
int TAP_ESC::send_packet(EscPacket &packet, int responder) { if (responder >= 0) { if (responder > _channels_count) { return -EINVAL; } select_responder(responder); } int packet_len = crc_packet(packet); int ret = ::write(_uart_fd, &packet.head, packet_len); if (ret != packet_len) { PX4_WARN("TX ERROR: ret: %d, errno: %d", ret, errno); } return ret; }
static void read_and_process(serial_source src, int non_blocking) /* Effects: reads and processes up to one packet. */ { uint8_t *packet = src->recv.packet; for (;;) { int byte = read_byte(src, non_blocking); if (byte < 0) return; if (!src->recv.in_sync) { if (byte == SYNC_BYTE) { src->recv.in_sync = TRUE; message(src, msg_sync); src->recv.count = 0; src->recv.escaped = FALSE; } continue; } if (src->recv.count >= MTU) { message(src, msg_too_long); src->recv.in_sync = FALSE; continue; } if (src->recv.escaped) { if (byte == SYNC_BYTE) { /* sync byte following escape is an error, resync */ message(src, msg_bad_sync); src->recv.in_sync = FALSE; continue; } byte ^= 0x20; src->recv.escaped = FALSE; } else if (byte == ESCAPE_BYTE) { src->recv.escaped = TRUE; continue; } else if (byte == SYNC_BYTE) { int count = src->recv.count; uint8_t *received; uint16_t read_crc, computed_crc; src->recv.count = 0; /* ready for next packet */ if (count < 4) /* frames that are too small are ignored */ continue; received = malloc(count - 2); if (!received) { message(src, msg_no_memory); continue; } memcpy(received, packet, count - 2); read_crc = packet[count - 2] | packet[count - 1] << 8; computed_crc = crc_packet(received, count - 2); #ifdef DEBUG dump("received", packet, count); printf(" crc %x comp %x\n", read_crc, computed_crc); #endif if (read_crc == computed_crc) { process_packet(src, received, count - 2); return; /* give rest of world chance to do something */ } else { message(src, msg_bad_crc); free(received); /* We don't lose sync here. If we did, garbage on the line at startup will cause loss of the first packet. */ continue; } } packet[src->recv.count++] = byte; } }
int PX4IO_serial_f4::_bus_exchange(IOPacket *_packet) { _current_packet = _packet; /* clear any lingering error status */ (void)rSR; (void)rDR; /* start RX DMA */ perf_begin(_pc_txns); perf_begin(_pc_dmasetup); /* DMA setup time ~3µs */ _rx_dma_status = _dma_status_waiting; /* * Note that we enable circular buffer mode as a workaround for * there being no API to disable the DMA FIFO. We need direct mode * because otherwise when the line idle interrupt fires there * will be packet bytes still in the DMA FIFO, and we will assume * that the idle was spurious. * * XXX this should be fixed with a NuttX change. */ stm32_dmasetup( _rx_dma, PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, reinterpret_cast<uint32_t>(_current_packet), sizeof(*_current_packet), DMA_SCR_CIRC | /* XXX see note above */ DMA_SCR_DIR_P2M | DMA_SCR_MINC | DMA_SCR_PSIZE_8BITS | DMA_SCR_MSIZE_8BITS | DMA_SCR_PBURST_SINGLE | DMA_SCR_MBURST_SINGLE); stm32_dmastart(_rx_dma, _dma_callback, this, false); rCR3 |= USART_CR3_DMAR; /* start TX DMA - no callback if we also expect a reply */ /* DMA setup time ~3µs */ stm32_dmasetup( _tx_dma, PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, reinterpret_cast<uint32_t>(_current_packet), PKT_SIZE(*_current_packet), DMA_SCR_DIR_M2P | DMA_SCR_MINC | DMA_SCR_PSIZE_8BITS | DMA_SCR_MSIZE_8BITS | DMA_SCR_PBURST_SINGLE | DMA_SCR_MBURST_SINGLE); stm32_dmastart(_tx_dma, nullptr, nullptr, false); //rCR1 &= ~USART_CR1_TE; //rCR1 |= USART_CR1_TE; rCR3 |= USART_CR3_DMAT; perf_end(_pc_dmasetup); /* compute the deadline for a 10ms timeout */ struct timespec abstime; clock_gettime(CLOCK_REALTIME, &abstime); abstime.tv_nsec += 10 * 1000 * 1000; if (abstime.tv_nsec >= 1000 * 1000 * 1000) { abstime.tv_sec++; abstime.tv_nsec -= 1000 * 1000 * 1000; } /* wait for the transaction to complete - 64 bytes @ 1.5Mbps ~426µs */ int ret; for (;;) { ret = sem_timedwait(&_completion_semaphore, &abstime); if (ret == OK) { /* check for DMA errors */ if (_rx_dma_status & DMA_STATUS_TEIF) { perf_count(_pc_dmaerrs); ret = -EIO; break; } /* check packet CRC - corrupt packet errors mean IO receive CRC error */ uint8_t crc = _current_packet->crc; _current_packet->crc = 0; if ((crc != crc_packet(_current_packet)) || (PKT_CODE(*_current_packet) == PKT_CODE_CORRUPT)) { perf_count(_pc_crcerrs); ret = -EIO; break; } /* successful txn (may still be reporting an error) */ break; } if (errno == ETIMEDOUT) { /* something has broken - clear out any partial DMA state and reconfigure */ _abort_dma(); perf_count(_pc_timeouts); perf_cancel(_pc_txns); /* don't count this as a transaction */ break; } /* we might? see this for EINTR */ syslog(LOG_ERR, "unexpected ret %d/%d\n", ret, errno); } /* reset DMA status */ _rx_dma_status = _dma_status_inactive; /* update counters */ perf_end(_pc_txns); return ret; }
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); } } }