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);
}
}