Exemplo n.º 1
0
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;
}
Exemplo n.º 2
0
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;
}
Exemplo n.º 3
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 */
}
Exemplo n.º 4
0
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;
}
Exemplo n.º 5
0
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;
}
Exemplo n.º 6
0
/** 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();
}
Exemplo n.º 8
0
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;
}
Exemplo n.º 9
0
// 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();
}
Exemplo n.º 10
0
 byte get1(void) {
   byte r = (pgm_read_byte_far(src) & mask) != 0;
   mask <<= 1;
   if (!mask) {
     mask = 1;
     src++;
   }
   return r;
 }
Exemplo n.º 11
0
// 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++;
	}
}
Exemplo n.º 12
0
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. */
}
Exemplo n.º 13
0
/** 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;
			}
		}
	}
Exemplo n.º 14
0
// 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++;
	}	
}
Exemplo n.º 15
0
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;
}
Exemplo n.º 16
0
//************************************************************************
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);
	}
}
Exemplo n.º 17
0
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;
}
Exemplo n.º 18
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);
		}
	}
}
Exemplo n.º 19
0
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
Exemplo n.º 20
0
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;
}
Exemplo n.º 21
0
/*
 * 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
}
Exemplo n.º 22
0
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
}
Exemplo n.º 23
0
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);
}
Exemplo n.º 24
0
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;		
}
Exemplo n.º 25
0
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;
}
Exemplo n.º 26
0
/* 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 */

}
Exemplo n.º 27
0
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;
							}
						}
					}
				}
			}
		}
	}
}
Exemplo n.º 28
0
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);
	}
}
Exemplo n.º 29
0
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;
}
Exemplo n.º 30
0
/* 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);
	}
}