float AP_Declination::get_declination(float lat, float lon) { int8_t decSW, decSE, decNW, decNE, lonmin, latmin; uint8_t latmin_index,lonmin_index; float decmin, decmax; // Constrain to valid inputs lat = constrain(lat, -90, 90); lon = constrain(lon, -180, 180); latmin = floor(lat/5)*5; lonmin = floor(lon/5)*5; latmin_index= (90+latmin)/5; lonmin_index= (180+lonmin)/5; decSW = (int8_t)pgm_read_byte_far(&dec_tbl[latmin_index][lonmin_index]); decSE = (int8_t)pgm_read_byte_far(&dec_tbl[latmin_index][lonmin_index+1]); decNE = (int8_t)pgm_read_byte_far(&dec_tbl[latmin_index+1][lonmin_index+1]); decNW = (int8_t)pgm_read_byte_far(&dec_tbl[latmin_index+1][lonmin_index]); /* approximate declination within the grid using bilinear interpolation */ decmin = (lon - lonmin) / 5 * (decSE - decSW) + decSW; decmax = (lon - lonmin) / 5 * (decNE - decNW) + decNW; return (lat - latmin) / 5 * (decmax - decmin) + decmin; }
uint16_t doPage(int page) { //char temp[50]; //TxDString("\n\rRequest for doPage\n\r"); if(page>127)page=127;//(MHD.CurrentPage != page) if(page!=-1){ // New request MHD.CurrentPage = page; cli(); MHD.NoOfPoses= pgm_read_byte_far ((uint32_t)POSE_BASE_ADR +(uint32_t)POSE_PAGE_SIZE*(uint32_t)MHD.CurrentPage + (uint32_t)POSE_PAGE_NUM_OF_MOTIONS); sei(); MHD.TotalTime=0; MHD.CurrentPose = 0; beep(); printf("new page %d: poses=%d\n", MHD.CurrentPage, MHD.NoOfPoses); int i; for(i=0;i<MHD.NoOfPoses;i++) { MHD.PosePause = doPose(MHD.CurrentPage,MHD.CurrentPose); //printf("last PosePause=%i\n",MHD.PosePause); while(motorIsMoving()) ; delay(MHD.PosePause); } return MHD.TotalTime; } return 0; }
void bootloader(void) { #define BOOTLOADER_ADDR 0x3f000 if (pgm_read_byte_far(BOOTLOADER_ADDR) == 0xff) { printf_P(PSTR("Bootloader is not present\r\n")); return; } cli(); BRAKE_ON(); /* ... very specific :( */ TIMSK0 = 0; TIMSK1 = 0; TIMSK2 = 0; TIMSK3 = 0; TIMSK4 = 0; TIMSK5 = 0; EIMSK = 0; UCSR0B = 0; UCSR1B = 0; UCSR2B = 0; UCSR3B = 0; SPCR = 0; TWCR = 0; ACSR = 0; ADCSRA = 0; EIND = 1; __asm__ __volatile__ ("ldi r31,0xf8\n"); __asm__ __volatile__ ("ldi r30,0x00\n"); __asm__ __volatile__ ("eijmp\n"); /* never returns */ }
void create_system_info_screen (void) { uint16_t addr; tft_clrscr(TFT_COLOR_LIGHTGRAY); // White before printf_tft_P( TFT_COLOR_BLUE, TFT_COLOR_WHITE, PSTR("Nut/OS %s "), NutVersionString()); printf_tft_P( TFT_COLOR_BLUE, TFT_COLOR_WHITE, PSTR("Firmware V%d.%d"), pgm_read_byte_far((char*)&bootlodrinfo.app_version +1), pgm_read_byte_far((char*)&bootlodrinfo.app_version)); printf_tft_P( TFT_COLOR_GREEN, TFT_COLOR_WHITE, PSTR("Build %s at %s"), __DATE__, __TIME__); addr = eib_get_device_address(EIB_DEVICE_CHANNEL); printf_tft_P( TFT_COLOR_RED, TFT_COLOR_WHITE, PSTR("Phys. Addr %d.%d.%d"), (addr >> 4) & 0x0f, addr & 0x0f, (addr >> 8) & 0xff ); if (display_orientation == DISPLAY_ORIENTATION_HOR) printf_tft_P( TFT_COLOR_BLACK, TFT_COLOR_WHITE, PSTR("Horizontal")); else if (display_orientation == DISPLAY_ORIENTATION_90L) printf_tft_P( TFT_COLOR_BLACK, TFT_COLOR_WHITE, PSTR("90 left")); else if (display_orientation == DISPLAY_ORIENTATION_90R) printf_tft_P( TFT_COLOR_BLACK, TFT_COLOR_WHITE, PSTR("90 right")); else if (display_orientation == DISPLAY_ORIENTATION_UPSIDE) printf_tft_P( TFT_COLOR_BLACK, TFT_COLOR_WHITE, PSTR("180")); if (eib_get_status() == EIB_NORMAL) printf_tft_P( TFT_COLOR_BLACK, TFT_COLOR_WHITE, PSTR("EIB online")); else printf_tft_P( TFT_COLOR_RED, TFT_COLOR_WHITE, PSTR("EIB offline")); printf_tft_P (TFT_COLOR_BLACK, TFT_COLOR_WHITE, PSTR("TFT Ctrl = %d, R00=%4.4x"), controller_type, controller_id); draw_button (ERASE_BUTTON_XPOS, ERASE_BUTTON_YPOS, BUTTON_WIDTH, "Erase Flash"); // added by sh draw_button (MONITOR_BUTTON_XPOS, MONITOR_BUTTON_YPOS, BUTTON_WIDTH, "Monitor"); draw_button (DOWNLOAD_BUTTON_XPOS, DOWNLOAD_BUTTON_YPOS, BUTTON_WIDTH, "Download"); draw_button (EXIT_BUTTON_XPOS, EXIT_BUTTON_YPOS, BUTTON_WIDTH, "Exit"); system_page_active = SYSTEM_PAGE_MAIN; }
void *poly_memcpy_PF(void *dest, uint_farptr_t src, size_t len) { uint8_t *ptr = dest; while (len--) { *ptr++ = pgm_read_byte_far(src++); } return dest; }
/** Reads or writes a block of EEPROM or FLASH memory to or from the appropriate CDC data endpoint, depending * on the AVR910 protocol command issued. * * \param[in] Command Single character AVR910 protocol command indicating what memory operation to perform */ static void ReadWriteMemoryBlock(const uint8_t Command) { uint16_t BlockSize; char MemoryType; bool HighByte = false; uint8_t LowByte = 0; BlockSize = (FetchNextCommandByte() << 8); BlockSize |= FetchNextCommandByte(); MemoryType = FetchNextCommandByte(); if ((MemoryType != 'E') && (MemoryType != 'F')) { /* Send error byte back to the host */ WriteNextResponseByte('?'); return; } /* Disable timer 1 interrupt - can't afford to process nonessential interrupts * while doing SPM tasks */ TIMSK1 = 0; /* Check if command is to read memory */ if (Command == 'g') { /* Re-enable RWW section */ boot_rww_enable(); while (BlockSize--) { if (MemoryType == 'F') { /* Read the next FLASH byte from the current FLASH page */ #if (FLASHEND > 0xFFFF) WriteNextResponseByte(pgm_read_byte_far(CurrAddress | HighByte)); #else WriteNextResponseByte(pgm_read_byte(CurrAddress | HighByte)); #endif /* If both bytes in current word have been read, increment the address counter */ if (HighByte) CurrAddress += 2; HighByte = !HighByte; } else { /* Read the next EEPROM byte into the endpoint */ WriteNextResponseByte(eeprom_read_byte((uint8_t*)(intptr_t)(CurrAddress >> 1))); /* Increment the address counter after use */ CurrAddress += 2; } } }
void GDClass::copy(unsigned int addr, uint_farptr_t src, int count) { __wstart(addr); while (count--) { SPI.transfer(pgm_read_byte_far(src)); src++; } __end(); }
byte vis_gd_fb_get1() { byte r = (pgm_read_byte_far(g_flashbits_src) & g_flashbits_mask) != 0; g_flashbits_mask <<= 1; if (!g_flashbits_mask) { g_flashbits_mask = 1; g_flashbits_src++; } return r; }
// copies arrays of bytes (GD background pic, char, pal) to the GD via spi void vis_gd_copy(unsigned int addr, uint_farptr_t src, int count) { vis_gd_wstart(addr); while (count--) { vis_spi_transfer(pgm_read_byte_far(src)); // Read a byte from the program space with a 32-bit (far) address and transfer it to the GD via spi src++; } vis_gd_end(); }
byte get1(void) { byte r = (pgm_read_byte_far(src) & mask) != 0; mask <<= 1; if (!mask) { mask = 1; src++; } return r; }
// read data from an offset in flash void boot_v_read_data( uint32_t offset, uint8_t *dest, uint16_t length ){ for(uint16_t i = 0; i < length; i++){ *dest = pgm_read_byte_far(offset); offset++; dest++; } }
void network_config_load (void) { /* load settings from eeprom */ #ifdef EEPROM_SUPPORT eeprom_restore (mac, uip_ethaddr.addr, 6); #else #if defined(BOOTLOADER_SUPPORT) && BOOTLOADER_START_ADDRESS > UINT16_MAX uint_farptr_t src = pgm_get_far_address (conf_mac); uint8_t *dst = uip_ethaddr.addr; for (uint8_t i = 6; i; i--) *dst++ = pgm_read_byte_far (src++); #else memcpy_P (uip_ethaddr.addr, conf_mac, 6); #endif #endif #if (defined(IPV4_SUPPORT) && !defined(BOOTP_SUPPORT) && !defined(DHCP_SUPPORT)) || defined(IPV6_STATIC_SUPPORT) uip_ipaddr_t ip; /* Configure the IP address. */ #ifdef EEPROM_SUPPORT /* Please Note: ip and &ip are NOT the same (cpp hell) */ eeprom_restore_ip (ip, &ip); #else set_CONF_ETHERRAPE_IP (&ip); #endif uip_sethostaddr (&ip); /* Configure prefix length (IPv6). */ #ifdef IPV6_SUPPORT uip_setprefixlen (CONF_ENC_IP6_PREFIX_LEN); #endif #ifdef IPV4_SUPPORT /* Configure the netmask (IPv4). */ #ifdef EEPROM_SUPPORT /* Please Note: ip and &ip are NOT the same (cpp hell) */ eeprom_restore_ip (netmask, &ip); #else set_CONF_ETHERRAPE_IP4_NETMASK (&ip); #endif uip_setnetmask (&ip); #endif /* IPV4_SUPPORT */ /* Configure the default gateway */ #ifdef EEPROM_SUPPORT /* Please Note: ip and &ip are NOT the same (cpp hell) */ eeprom_restore_ip (gateway, &ip); #else set_CONF_ETHERRAPE_GATEWAY (&ip); #endif uip_setdraddr (&ip); #endif /* No autoconfiguration. */ }
/** Reads or writes a block of EEPROM or FLASH memory to or from the appropriate CDC data endpoint, depending * on the AVR109 protocol command issued. * * \param[in] Command Single character AVR109 protocol command indicating what memory operation to perform */ static void ReadWriteMemoryBlock(const uint8_t Command) { uint16_t BlockSize; char MemoryType; uint8_t HighByte = 0; uint8_t LowByte = 0; BlockSize = (FetchNextCommandByte() << 8); BlockSize |= FetchNextCommandByte(); MemoryType = FetchNextCommandByte(); if ((MemoryType != MEMORY_TYPE_FLASH) && (MemoryType != MEMORY_TYPE_EEPROM)) { /* Send error byte back to the host */ WriteNextResponseByte('?'); return; } /* Check if command is to read a memory block */ if (Command == AVR109_COMMAND_BlockRead) { /* Re-enable RWW section */ boot_rww_enable(); while (BlockSize--) { if (MemoryType == MEMORY_TYPE_FLASH) { /* Read the next FLASH byte from the current FLASH page */ #if (FLASHEND > 0xFFFF) WriteNextResponseByte(pgm_read_byte_far(CurrAddress | HighByte)); #else WriteNextResponseByte(pgm_read_byte(CurrAddress | HighByte)); #endif /* If both bytes in current word have been read, increment the address counter */ if (HighByte) CurrAddress += 2; HighByte = !HighByte; } else { /* Read the next EEPROM byte into the endpoint */ WriteNextResponseByte(eeprom_read_byte((uint8_t*)(intptr_t)(CurrAddress >> 1))); /* Increment the address counter after use */ CurrAddress += 2; } } }
// read specified flash page into destination pointer void boot_v_read_page( uint16_t pagenumber, uint8_t *dest ){ uint32_t addr = ( uint32_t )pagenumber * ( uint32_t )PAGE_SIZE; for(uint16_t i = 0; i < PAGE_SIZE; i++){ *dest = pgm_read_byte_far(addr); addr++; dest++; } }
static void* memcpy_PF (uint8_t *dst, uint_farptr_t src, size_t len) { uint8_t *dst_save = dst; while (len--) { *dst = pgm_read_byte_far (src); src++; dst++; } return dst_save; }
//************************************************************************ void Serial_print_P(prog_char *flashMemStr) { char theChar; int ii; ii = 0; #if (FLASHEND > 0x10000) while (theChar = pgm_read_byte_far(flashMemStr + ii++)) #else while (theChar = pgm_read_byte_near(flashMemStr + ii++)) #endif { Serial.print(theChar); } }
static int memcmp_PF(const uint8_t *s1, uint_farptr_t s2, size_t len) { while (len--) { uint8_t c = pgm_read_byte_far (s2); if (*s1 < c) return -1; else if (*s1 > c) return 1; s1++; s2++; } return 0; }
void flash_read( uint32_t address, void *buf, uint16_t size ) { uint16_t i = 0; if( (address % 2) == 0 && (size % 2) == 0 ) { uint16_t *wp = (uint16_t*)buf; for(i = 0; i < size; i+=2){ wp[i/2] = pgm_read_word_far(address + i); } } else { uint8_t *data = (uint8_t*)buf; for(i = 0; i < size; i+=1){ data[i] = pgm_read_byte_far(address + i); } } }
void PutStr_P( const prog_char *str ) { uint8_t ch; while (( ch = pgm_read_byte_far( str )) != '\0' ) { if ( ch == '\n' ) { UART0_PutChar( '\r' ); } UART0_PutChar( ch ); str++; } } // PutStr_P
void ws_flash_read_byte(uint32_t addr, uint8_t *buf) { uint8_t sreg; sreg = SREG; cli(); eeprom_busy_wait(); _WAIT_FOR_SPM(); eeprom_busy_wait(); boot_spm_busy_wait(); _ENABLE_RWW_SECTION(); *buf = pgm_read_byte_far(addr); boot_rww_enable(); SREG = sreg; return; }
/* * Read from flash info buf. addr contains starting flash byte address */ void avr_flash_read(CFS_CONF_OFFSET_TYPE addr, uint8_t *buf, CFS_CONF_OFFSET_TYPE size) { uint32_t addr32=COFFEE_START+addr; uint16_t isize=size; #if DEBUG unsigned char *bufo=(unsigned char *)buf; uint8_t i; uint16_t w=addr32>>1; //Show progmem word address for debug PRINTF("r0x%04x(%u) ",w,size); #endif #ifndef FLASH_WORD_READS for (;isize>0;isize--) { #if FLASH_COMPLEMENT_DATA *buf++=~(uint8_t)pgm_read_byte_far(addr32++); #else *buf++=(uint8_t)pgm_read_byte_far(addr32++); #endif /*FLASH_COMPLEMENT_DATA*/ } #else /* 130 bytes more PROGMEM, but faster */ if (isize&0x01) { //handle first odd byte #if FLASH_COMPLEMENT_DATA *buf++=~(uint8_t)pgm_read_byte_far(addr32++); #else *buf++=(uint8_t)pgm_read_byte_far(addr32++); #endif /*FLASH_COMPLEMENT_DATA*/ isize--; } for (;isize>1;isize-=2) {//read words from flash #if FLASH_COMPLEMENT_DATA *(uint16_t *)buf=~(uint16_t)pgm_read_word_far(addr32); #else *(uint16_t *)buf=(uint16_t)pgm_read_word_far(addr32); #endif /*FLASH_COMPLEMENT_DATA*/ buf+=2; addr32+=2; } if (isize) { //handle last odd byte #if FLASH_COMPLEMENT_DATA *buf++=~(uint8_t)pgm_read_byte_far(addr32); #else *buf++=(uint8_t)pgm_read_byte_far(addr32); #endif /*FLASH_COMPLEMENT_DATA*/ } #endif /* FLASH_WORD_READS */ #if DEBUG>1 PRINTF("\nbuf="); // PRINTF("%s",bufo); // for (i=0;i<16;i++) PRINTF("%2x ",*bufo++); #endif }
void vis_gd_ascii() { long i; for (i = 0; i < 768; i++) { #if defined(__AVR_ATmega1284P__) || defined(__AVR_AT90USB1286__) byte b = pgm_read_byte_far(GET_FAR_ADDRESS(font8x8) + i); #else byte b = pgm_read_byte(font8x8 + i); #endif byte h = stretch[b >> 4]; byte l = stretch[b & 0xf]; vis_gd_wr(0x1000 + (16 * ' ') + (2 * i), h); // RAM_CHR offset vis_gd_wr(0x1000 + (16 * ' ') + (2 * i) + 1, l); } for (i = 0x20; i < 0x80; i++) { vis_gd_setpal(4 * i + 0, TRANSPARENT); vis_gd_setpal(4 * i + 3, RGB(255,255,255)); // white color } vis_gd_fill(RAM_PIC, ' ', 4096); // write empty spaces }
void GDClass::ascii() { long i; for (i = 0; i < 768; i++) { #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) byte b = pgm_read_byte_far(GET_FAR_ADDRESS(font8x8) + i); #else byte b = pgm_read_byte(font8x8 + i); #endif byte h = stretch[b >> 4]; byte l = stretch[b & 0xf]; GD.wr(0x1000 + (16 * ' ') + (2 * i), h); GD.wr(0x1000 + (16 * ' ') + (2 * i) + 1, l); } for (i = 0x20; i < 0x80; i++) { GD.setpal(4 * i + 0, TRANSPARENT); GD.setpal(4 * i + 3, RGB(255,255,255)); } GD.fill(RAM_PIC, ' ', 4096); }
uint16_t ldr_u16_get_internal_crc( void ){ static uint16_t crc; crc = 0xffff; uint32_t length = ldr_u32_read_internal_length(); for( uint32_t i = 0; i < length; i++ ){ crc = crc_u16_byte( crc, pgm_read_byte_far( i ) ); // reset watchdog timer wdt_reset(); } crc = crc_u16_byte( crc, 0 ); crc = crc_u16_byte( crc, 0 ); return crc; }
void ws_flash_read_page(uint32_t addr, uint8_t *buf) { uint8_t sreg; uint32_t i; sreg = SREG; cli(); eeprom_busy_wait(); for (i=0;i<PAGESIZE;i++) { _WAIT_FOR_SPM(); eeprom_busy_wait(); boot_spm_busy_wait(); _ENABLE_RWW_SECTION(); buf[i] = pgm_read_byte_far(addr+i); } boot_rww_enable(); SREG = sreg; return; }
/* main program starts here */ void bootloader(void) { #ifdef ADABOOT char firstcharzero=0; #endif uint8_t ch,ch2; uint16_t w; error_count = 0; initSerial(); /* forever loop */ for (;;) { bitTgl(UsbLed); if (error_count >= MAX_ERROR_COUNT) // Probably normal serial traffic is causing this. { releaseChipAccess(); return; //initSerial(); //error_count = 0; } /* get character from UART */ //bitSet(BluLed); ch = bootldrgetch(); if (ch ==0) { releaseChipAccess(); return; } //bitClr(BluLed); //blinkLed(GrnLedId,100*mSec,1); /* A bunch of if...else if... gives smaller code than switch...case ! */ #if 0 //def ADABOOT // Hello is anyone home ? lady ada hack - BBR if(ch=='0') { firstcharzero = 1; // we got an appropriate bootloader instruction nothing_response(); } else if (firstcharzero == 0) { // the first character we got is not '0', lets bail! // autoreset via watchdog (sneaky!) // return(); } #else /* Hello is anyone home ? */ if(ch=='0') { nothing_response(); } #endif /* Request programmer ID */ /* Not using PROGMEM string due to boot block in m128 being beyond 64kB boundry */ /* Would need to selectively manipulate RAMPZ, and it's only 9 characters anyway so who cares. */ else if(ch=='1') { if (bootldrgetch() == ' ') { putNch(pgmrId,9); } else { ++error_count; } } /* AVR ISP/STK500 board commands DON'T CARE so default nothing_response */ else if(ch=='@') { ch2 = bootldrgetch(); if (ch2>0x85) bootldrgetch(); nothing_response(); } /* AVR ISP/STK500 board requests */ /* Cmnd_STK_GET_PARAMETER 0x41 */ else if(ch=='A') { ch2 = bootldrgetch(); if(ch2==0x80) byte_response(HW_VER); // Hardware version else if(ch2==0x81) byte_response(SW_MAJOR); // Software major version else if(ch2==0x82) byte_response(SW_MINOR); // Software minor version else if(ch2==0x98) byte_response(0x03); // Unknown but seems to be required by avr studio 3.56 else byte_response(0x00); // Covers various unnecessary responses we don't care about } /* Cmnd_STK_SET_DEVICE 0x42 Device Parameters DON'T CARE, DEVICE IS FIXED */ else if(ch=='B') { getNch(20); nothing_response(); } /* Cmnd_SET_DEVICE_EXT 0x45 */ else if(ch=='E') { getNch(5); nothing_response(); } /* P: Enter programming mode */ else if(ch=='P') { if (!initChipAccess(&devId[1])) { failed_response(); } else nothing_response(); } /* R: Erase device, don't care as we will erase one page at a time anyway. */ else if(ch=='R') { eraseDevice(); nothing_response(); } /* Leave programming mode */ else if(ch=='Q') { nothing_response(); delay(5*mSec); releaseChipAccess(); allDone(); return; #ifdef WATCHDOG_MODS assert(0); #endif } /* Set address, little endian. EEPROM in bytes, FLASH in words */ /* Perhaps extra address bytes may be added in future to support > 128kB FLASH. */ /* This might explain why little endian was used here, big endian used everywhere else. */ else if(ch=='U') { address.word = myget2LE(); //address.byte[0] = bootldrgetch(); //address.byte[1] = bootldrgetch(); nothing_response(); } /* Universal SPI programming command. Used for fuses and lock bits. */ else if(ch=='V') { unsigned char spicmdbuf[4]; bootldrgetNch(spicmdbuf,4); if (0) { // spicmdbuf[0] == 0x30) { if (spicmdbuf[2] == 0) { byte_response(SIG1); } else if (spicmdbuf[2] == 1) { byte_response(SIG2); } else { byte_response(SIG3); } } // Call my erase device embedded routine if the erase device SPI cmd comes in else if ((spicmdbuf[0] == 0xac) && (spicmdbuf[1] == 0x80)) { eraseDevice(); byte_response(0); } // Don't let them program the fuse bits into a non-working clock // This SPI on this chip can't go slow enough to work with very slow AVR clocks (<=1mhz) else if ((spicmdbuf[0] == 0xAC) && (spicmdbuf[1] == 0xA0) && (((spicmdbuf[3]&0x80) == 0) || ((spicmdbuf[3]&0x0f) == 3))) { failed_response(); } else { byte_response(issueSpiCmd(spicmdbuf)); } } /* Write memory, length is big endian and is in bytes */ else if(ch=='d') { length.word = myget2BE(); //length.byte[1] = bootldrgetch(); //length.byte[0] = bootldrgetch(); flags.eeprom = 0; if (bootldrgetch() == 'E') flags.eeprom = 1; for (w=0;w<length.word;w++) { buff[w] = bootldrgetch(); // Store data in buffer, can't keep up with serial data stream whilst programming pages } if (bootldrgetch() == ' ') { if (flags.eeprom) { //Write to EEPROM one byte at a time address.word <<= 1; for(w=0;w<length.word;w++) { eeprom_write_byte((void *)address.word,buff[w]); address.word++; } } else { //Write to FLASH one page at a time if (address.byte[1]>127) address_high = 0x01; //Only possible with m128, m256 will need 3rd address byte. FIXME else address_high = 0x00; address.word = address.word << 1; //address * 2 -> byte location /* if ((length.byte[0] & 0x01) == 0x01) length.word++; //Even up an odd number of bytes */ //if ((length.byte[0] & 0x01)) length.word++; //Even up an odd number of bytes if (length.word&1) length.word++; // Even up an odd number of bytes flash_write((void*)address.word, length.word, address_high, buff); } putNch(endStmt,2); //myputch(0x14); //myputch(0x10); } else { ++error_count; } } /* Read memory block mode, length is big endian. */ else if(ch=='t') { length.word = myget2BE(); //length.byte[1] = bootldrgetch(); //length.byte[0] = bootldrgetch(); #if defined(__AVR_ATmega128__) || defined(__AVR_ATmega1280__) if (address.word>0x7FFF) flags.rampz = 1; // No go with m256, FIXME else flags.rampz = 0; #endif address.word = address.word << 1; // address * 2 -> byte location if (bootldrgetch() == 'E') flags.eeprom = 1; else flags.eeprom = 0; if (bootldrgetch() == ' ') { // Command terminator myputch(0x14); if (1) { // Can handle odd and even lengths okay if (flags.eeprom) { // Byte access EEPROM read for (w=0;w < length.word;w++,address.word++) { myputch(eeprom_read_byte((void *)address.word)); } } else { int bpos = 0; for (w=0;w < length.word;w++,address.word++,bpos++) { if (!flags.rampz) buff[bpos] = pgm_read_byte_near((void*)address.word); #if defined(__AVR_ATmega128__) || defined(__AVR_ATmega1280__) else buff[bpos] = pgm_read_byte_far(address.word + 0x10000); // Hmmmm, yuck FIXME when m256 arrvies #endif if (bpos == 63) { putNch(buff,64); bpos = -1; } } if (bpos > 0) { putNch(buff,bpos); } } } myputch(0x10); } } /* Get device signature bytes */ else if(ch=='u') { if (bootldrgetch() == ' ') { bitSet(UsbLed); putNch(devId,5); } else { ++error_count; } } /* Read oscillator calibration byte */ else if(ch=='v') { byte_response(0x00); } #ifdef MONITOR /* here come the extended monitor commands by Erik Lins */ /* check for three times exclamation mark pressed */ else if(ch=='!') monitor(); #endif else /* Garbled 1st character */ { //printf("garbled character %d %c\n",ch,ch); ++error_count; } } /* end of forever loop */ }
void ContinueMotion_HeartBeat(void) { uint8_t MOVING = FALSE; if(MHD.CurrentPage != -1){ // Check if Servos are still in Motion /* gbpParameter[0] = P_MOVING; gbpParameter[1] = 1; //Read Length for( ServoNo=1;ServoNo < NUM_OF_SERVOS_ATTACHED; ServoNo++){ TxPacket(ServoNo,INST_READ,2); uint8_t bRxPacketLength = RxPacket(DEFAULT_RETURN_PACKET_SIZE+gbpParameter[1]); if(bRxPacketLength == DEFAULT_RETURN_PACKET_SIZE+gbpParameter[1]){ if(gbpRxBuffer[4]==0){ //TxDString("M:");TxD8Hex(gbpRxBuffer[5]); if(gbpRxBuffer[5]!=0){ // Servo is still moving MOVING = TRUE; } }else{ TxDString("Error:");//gbpRxBuffer[4] } } } */ if(MHD.MoveFinishTime>0){MHD.MoveFinishTime--;} if(MHD.MoveFinishTime==0){ MOVING = FALSE; //TxDString("M"); }else{ MOVING = TRUE; } if(MOVING == FALSE){ if(MHD.PosePause>0){MHD.PosePause--;} if(MHD.PosePause==0){ //TxDString("+\n\r"); if(ROBOTSTATUS!=STOP_POSE){ if(MHD.CurrentPose+1<MHD.NoOfPoses){ // next pose MHD.CurrentPose++; //TxDString("---Next Pose---\n\r"); MHD.PosePause = doPose(MHD.CurrentPage,MHD.CurrentPose); }else{ // finished //TxDString("Done Page\n\r"); //printf("[pg%02X]\n",MHD.CurrentPage); #ifdef ROBOT_DASH_MODE if(MHD.CurrentPage==FORWARD_STEP_COMPLETED_PAGE_NO){ Forward_Dash_StepNo++; } if(MHD.CurrentPage==BACKWARD_STEP_COMPLETED_PAGE_NO){ Backward_Dash_StepNo++; } #endif cli(); uint8_t NextPage= pgm_read_byte_far ((uint32_t)POSE_BASE_ADR + (uint32_t)POSE_PAGE_SIZE*(uint32_t)MHD.CurrentPage + (uint32_t)POSE_PAGE_NEXT_PAGE); sei(); if(ROBOTSTATUS!=STOP_PAGE){ //TxDString("NEXT PAGE\n\r");//NextPage; if(NextPage != 0){ doPage(NextPage); }else{ MHD.CurrentPage=-1; } }else{ uint8_t ExitPage= pgm_read_byte_far ((uint32_t)POSE_BASE_ADR + (uint32_t)POSE_PAGE_SIZE*(uint32_t)MHD.CurrentPage + (uint32_t)POSE_PAGE_EXIT_PAGE); if(ExitPage != 0){ doPage(ExitPage); }else{ MHD.CurrentPage=-1; } } } } } } } }
void stubboot_read_table(struct stubboot_table *t) { for (uint8_t i = 0; i < sizeof(*t); i++) { ((uint8_t *)t)[i] = pgm_read_byte_far(STUBBOOT_ADDR + i); } }
uint16_t doPose(int page,char pose) { int i; uint32_t pageAddr = POSE_BASE_ADR + page*POSE_PAGE_SIZE; uint32_t poseAddr = pageAddr + POSE_0_OFFSET + pose*POSE_SIZE; uint16_t PageSpeed = pgm_read_byte_far(pageAddr + POSE_PAGE_MOTION_SPEED); uint16_t PageFast = pgm_read_byte_far(pageAddr + POSE_PAGE_FAST_FLAG); uint16_t PosePause = pgm_read_byte_far(poseAddr + POSE_PAUSE_ADR); uint8_t PoseSpeed = pgm_read_byte_far(poseAddr + POSE_SPEED_ADR); struct { uint16_t pos; uint16_t dist; } servoControl[NUM_OF_SERVOS_ATTACHED]; //printf("PageSpeed: %d, PoseSpeed: %d\n", PageSpeed, PoseSpeed); if(MHD.CurrentPage!=-1){ cli(); uint8_t NoOfPosesinPage = pgm_read_byte_far (pageAddr + POSE_PAGE_NUM_OF_MOTIONS); sei(); if( pose+1 > NoOfPosesinPage){ //printf("E:Pose not on this page\n\r"); return 0; } cli(); #ifdef ROBOT_DASH_MODE if((PageFast==10)&&(PageSpeed>=10)){PageSpeed=280;} #else if((PageFast==10)&&(PageSpeed>=10)){PageSpeed=160;} #endif sei(); //uint8_t AccelTime = pgm_read_byte_far ((uint32_t)POSE_BASE_ADR + (uint32_t)POSE_PAGE_SIZE*(uint32_t)page + (uint32_t)POSE_PAGE_ACCEL_TIME); //uint16_t Ta_T0 = (float)AccelTime*k; //sprintf(temp,"\r\nPageNo=%i,PageSpeed=%i,PoseSpeed=%i,PosePause=%i\r\n",MHD.CurrentPage,PageSpeed,PoseSpeed,PosePause);TxDString(temp); //sprintf(temp,"#%i,%i Sp%i,%i,Pa%i\n\r",page,pose,PageSpeed,PoseSpeed,PosePause);TxDString(temp); /** Find the Servo that has to move the longest path (ThetaMax) This servo will be the only one that moves at the specified speed The other servos will move slower so that all servos finish the move at the same time */ uint16_t longestDistance = 0; for(i=1;i < NUM_OF_SERVOS_ATTACHED; i++){ if(ServoPos[i] != SERVO_NOT_CONNECTED){ cli(); int16_t targetPos = pgm_read_word_far(poseAddr + (uint32_t)i*2 ); sei(); //ThetaTarget += ServoTrim[i]; if(targetPos < 0) { //printf("pos clipped from %d\n", targetPos); targetPos = 0; } else if(targetPos > 1023 ) { //printf("pos clipped from %d\n", targetPos); targetPos = 1023; } uint16_t distance = abs((int16_t)targetPos - (int16_t)ServoPos[i]); servoControl[i].pos = targetPos; servoControl[i].dist = distance; if(distance > longestDistance){ longestDistance = distance; } } } /** Calculate Omega-PageSpeed-PoseSpeed-Normalised: Servos move with 90 deg/sec when PageSpeed=32 and PoseSpeed=32 At this point OmegaPPN is 1.00 */ float OmegaPPN = ((float)(PageSpeed*PoseSpeed))/1024.0; int i; for(i=1;i < NUM_OF_SERVOS_ATTACHED; i++){ //printf("dist %d / dist %d, max %d\n", servoControl[i].dist, longestDistance); // calculate servo speed double speed = (double)servoControl[i].dist/(double)longestDistance; speed *= (double)505.67300322796609L; if(speed < 0) speed = 10; SendServoTargetPos(i,servoControl[i].pos,(int)speed); ServoPos[i] = servoControl[i].pos; // remember new target pose } /** Calculate how many time steps it will take to finish the move. The servo that moves furthest is used with the formulae t = theta/omega The conversion factors have to be taken into account: - for ThetaMax: one degree in servo position is 3.41 servo units - for OmegaPPN: OmegaPPN is 1 at 90 deg/sec Using the conversion above gives the time in seconds - in order to change to timesteps divide by 7.8msec All in all results in 0.41707 */ MHD.MoveFinishTime = (uint16_t)(0.41707*((float)longestDistance/OmegaPPN)); if(MHD.MoveFinishTime > 30*128){printf("[E:FinishTime too long?]");} //sprintf(temp,"MFT=%i,TM=%i,OPPM=%i\n\r",MHD.MoveFinishTime,ThetaMax,(int)(OmegaPPN*1024.0));TxDString(temp); /* if PageSpeed is set to 32, a Pause in the Pose of 128 will last 1 second */ PosePause = (PosePause * 32)/PageSpeed; }else{ // Page is -1 return 0; } return PosePause; }
/* main program starts here */ void optiboot(void) { uint8_t ch; /* Forever loop */ for (;;) { /* get character from UART */ ch = getch(); uint8_t reply = STK_OK; if(ch == STK_GET_PARAMETER) { // GET PARAMETER returns a generic 0x03 reply - enough to keep Avrdude happy getNch(1); putch(0x03); } else if(ch == STK_SET_DEVICE) { // SET DEVICE is ignored getNch(20); } else if(ch == STK_SET_DEVICE_EXT) { // SET DEVICE EXT is ignored getNch(5); } else if(ch == STK_LOAD_ADDRESS) { // LOAD ADDRESS uint16_t address = getch(); address = (address & 0xff) | (getch() << 8); verifySpace(); // address is in words right now if (address % (SPM_PAGESIZE / 2)) { reply = STK_FAILED; } else { // Convert to page address page = address / (SPM_PAGESIZE / 2); } } else if(ch == STK_UNIVERSAL) { // UNIVERSAL command is ignored getNch(4); putch(0x00); } /* Write memory, length is big endian and is in bytes */ else if(ch == STK_PROG_PAGE) { // PROGRAM PAGE uint8_t *bufPtr = buff; // we support flash programming only, not EEPROM if ((getLen() == 'E') || (length > SPM_PAGESIZE) || (page >= (CONFIG_BOOTLDR_START_ADDR / SPM_PAGESIZE))) { reply = STK_FAILED; } // Clear the buffer memset(buff, 0xff, sizeof(buff)); // Read in page contents do *bufPtr++ = getch(); while (--length); // Read command terminator, start reply verifySpace(); if (reply == STK_OK) { // Write the page stubboot_write_page(page, buff); } } /* Read memory block mode, length is big endian. */ else if(ch == STK_READ_PAGE) { // READ PAGE - we only read flash if (getLen() == 'E') { reply = STK_FAILED; } verifySpace(); #if (FLASHEND > USHRT_MAX) uint_farptr_t address = (uint_farptr_t)page * SPM_PAGESIZE; #else uint16_t address = page * SPM_PAGESIZE; #endif while (length--) { #if (FLASHEND > USHRT_MAX) putch(pgm_read_byte_far(address++)); #else putch(pgm_read_byte_near(address++)); #endif } } /* Get device signature bytes */ else if(ch == STK_READ_SIGN) { // READ SIGN - return what Avrdude wants to hear verifySpace(); putch(SIGNATURE_0); putch(SIGNATURE_1); putch(SIGNATURE_2); } else { // This covers the response to commands like STK_ENTER_PROGMODE verifySpace(); } putch(reply); } }