Пример #1
0
// FlashPage writes 1 page to Crazyflie flash storage
func FlashPage(dev cflie.Device, info Info, page int, mem []byte) (err error) {
	if len(mem) != info.PageSize {
		return fmt.Errorf("FlashPage: %d = len(mem) != info.PageSize = %d", len(mem), info.PageSize)
	}
	if page < info.FlashStart {
		return fmt.Errorf("FlashPage: %d = page < FlashStart =  %d", page, info.FlashStart)
	}
	if page >= info.FlashPages {
		return fmt.Errorf("FlashPage: %d = page >= info.FlashPages = %d", page, info.FlashPages)
	}
	buf := make([]byte, 128)
	got := make(map[int]bool)

	// 1. Load page to memory buffer and verify that all the data is correct
	for try := 0; try < 10; try++ {
		// Load buffer
		for offset := 0; offset < info.PageSize; offset += 16 {
			if got[offset] {
				// Skip chunks which are already in the buffer
				continue
			}
			_, err = dev.Write(loadBuffer(0, uint16(offset), mem[offset:offset+16]))
			if err != nil {
				log.Printf("write: %v", err)
			}
		}

		// Read buffer
		for rtry := 0; rtry <= 2; rtry++ {
			for offset := 0; offset < info.PageSize; offset += 16 {
				if got[offset] {
					continue
				}
				_, err = dev.Write(readBuffer(0, uint16(offset)))
				if err != nil {
					log.Printf("write: %v", err)
					continue
				}
				n, err := dev.Read(buf)
				if err != nil {
					log.Printf("read: n: %d, err: %v", n, err)
					continue
				}
				if n == 0 {
					log.Printf("Empty packet")
					continue
				}
				// First byte is auxiliary
				p := buf[1:n]
				if len(p) < 10 || p[2] != CMD_READ_BUFFER {
					// Some weird packet; ignore it
					continue
				}
				inPage := int(p[3]) + (int(p[4]) << 8)
				if inPage != 0 {
					log.Printf("%d = inPage != 0", inPage)
					continue
				}
				inOffset := int(p[5]) + (int(p[6]) << 8)
				inData := p[7 : 7+16]
				// Check that the contents are correct
				ok := true
				for i, v := range inData {
					if mem[inOffset+i] != v {
						log.Printf("Chunk with incorrect data detected, offset=%d",
							inOffset)
						ok = false
						break
					}
				}
				if ok {
					got[inOffset] = true
				}
			}
		}
	}
	// Check that we got all chunks to the buffer
	ok := true
	for offset := 0; offset < info.PageSize; offset += 16 {
		if !got[offset] {
			log.Printf("Failed to write a chunk into a buffer, offset=%d", offset)
			ok = false
		}
	}
	if !ok {
		return fmt.Errorf("Some chunks failed to be loaded into Crazyflie memory buffer")
	}
	log.Printf("Data for page #%d loaded into Crazyflie memory buffer", page)

	// 2. Write from memory buffer to Flash
	for try := 0; try < 10; try++ {
		_, err := dev.Write(writeFlash(0, uint16(page), 1))
		if err != nil {
			log.Printf("Unable to send CMD_WRITE_FLASH packet: %v", err)
		}
		deadline := time.Now().Add(time.Second)
		ok := false
		for time.Now().Before(deadline) {
			dev.Write([]byte{0xFF})
			n, err := dev.Read(buf)
			if err != nil {
				log.Printf("read: %v", err)
				continue
			}
			// First byte is auxiliary
			p := buf[1:n]
			if len(p) < 4 || p[2] != CMD_WRITE_FLASH {
				// Some weird packet; ignore it
				continue
			}
			if p[3] != 1 /* done */ || p[4] != 0 /* error */ {
				log.Printf("Flashing attempt failed, done: %d, error: %d", p[3], p[4])
				continue
			}
			ok = true
			break
		}
		if ok {
			break
		}
	}
	log.Printf("Page %d seems to be written, verifying...", page)

	// 3. Read Flash page and verify
	dump, err := Dump(dev, info, page, page+1)
	if err != nil {
		return fmt.Errorf("Failed to dump the contents of page #%d: %v", page, err)
	}
	if !bytes.Equal(mem, dump) {
		return fmt.Errorf("Page #%d has unexpected contents", page)
	}
	return nil
}
Пример #2
0
// Dump downloads a region of Flash memory from Crazyflie. Device must be already connected to the bootloader.
func Dump(dev cflie.Device, info Info, fromPage, toPage int) (mem []byte, err error) {
	buf := make([]byte, 128)
	got := make(map[int]bool)
	mem = make([]byte, (toPage-fromPage)*info.PageSize)
	for try := 0; try < 10; try++ {
		for page := fromPage; page < toPage; page++ {
			if try == 0 {
				fmt.Fprintf(os.Stderr, ".")
			}
			for offset := 0; offset < info.PageSize; offset += 16 {
				start := page*info.PageSize + offset
				if got[start] {
					// Do not request already received chunks
					continue
				}
				if try > 0 {
					fmt.Fprintf(os.Stderr, "{Retry: %d}", start)
				}
				_, err = dev.Write(readFlash(uint16(page), uint16(offset)))
				if err != nil {
					log.Printf("write: %v", err)
					continue
				}
				n, err := dev.Read(buf)
				if err != nil {
					log.Printf("read: n: %d, err: %v", n, err)
					continue
				}
				if n == 0 {
					log.Printf("Empty packet")
					continue
				}
				p := buf[1:n]
				if len(p) > 10 && p[2] == CMD_READ_FLASH {
					page := int(p[3]) + (int(p[4]) << 8)
					offset := int(p[5]) + (int(p[6]) << 8)
					data := p[7 : 7+16]
					start := page*info.PageSize + offset
					got[start] = true
					index := start - fromPage*info.PageSize
					copy(mem[index:index+16], data)

				}
			}
		}
	}

	missing := false
	for page := fromPage; page < toPage; page++ {
		for offset := 0; offset < info.PageSize; offset += 16 {
			start := page*info.PageSize + offset
			if !got[start] {
				log.Printf("Missing chunk: index=%d", start)
				missing = true
			}
		}
	}
	fmt.Fprintf(os.Stderr, "\n")
	if missing {
		return nil, fmt.Errorf("Some chunks are failed to download")
	}
	return
}