void ST7920LCDDriver::drawText(uint8_t x,uint8_t y,uint8_t *char4) {
uint8_t p1,p2;
for (uint8_t r = 0;r < 8;r++) {
p1 = pgm_read_word_far(__fontData+(char4[0]*8+r));
p2 = pgm_read_word_far(__fontData+(char4[1]*8+r));
draw(x,y++,(p1 << 8) | p2);
}
}
/**
* Cycle all patterns and search matching pattern. Execute command callback.
* @param context
* @result TRUE if context->paramlist is filled with correct values
*/
static scpi_bool_t findCommandHeader(scpi_t * context, const char * header, int len) {
int32_t i;
#if USE_64K_PROGMEM_FOR_CMD_LIST
PGM_P pattern;
for (i = 0; (pattern = (PGM_P)pgm_read_word(&context->cmdlist[i].pattern)) != 0; ++i) {
strncpy_P(context->param_list.cmd_pattern_s, pattern, SCPI_MAX_CMD_PATTERN_SIZE);
context->param_list.cmd_pattern_s[SCPI_MAX_CMD_PATTERN_SIZE] = '\0';
if (matchCommand(context->param_list.cmd_pattern_s, header, len, NULL, 0, 0)) {
context->param_list.cmd_s.callback = (scpi_command_callback_t)pgm_read_word(&context->cmdlist[i].callback);
#if USE_COMMAND_TAGS
context->param_list.cmd_s.tag = (int32_t)pgm_read_dword(&context->cmdlist[i].tag);
#endif
return TRUE;
}
}
#elif USE_FULL_PROGMEM_FOR_CMD_LIST
uint_farptr_t p_cmd = context->cmdlist;
uint_farptr_t p_pattern = context->cmdpatterns;
uint16_t pattern_length;
for (i = 0;
(pattern_length = pgm_read_word_far(p_cmd + offsetof(scpi_command_t, pattern))) != 0;
++i, p_cmd += sizeof(scpi_command_t), p_pattern += pattern_length)
{
strncpy_PF(context->param_list.cmd_pattern_s, p_pattern, pattern_length);
context->param_list.cmd_pattern_s[pattern_length] = '\0';
if (matchCommand(context->param_list.cmd_pattern_s, header, len, NULL, 0, 0)) {
context->param_list.cmd_s.callback = (scpi_command_callback_t)pgm_read_word_far(p_cmd + offsetof(scpi_command_t, callback));
#if USE_COMMAND_TAGS
context->param_list.cmd_s.tag = (int32_t)pgm_read_dword_far(p_cmd + offsetof(scpi_command_t, tag));
#endif
return TRUE;
}
}
#else
const scpi_command_t * cmd;
for (i = 0; context->cmdlist[i].pattern != NULL; i++) {
cmd = &context->cmdlist[i];
if (matchCommand(cmd->pattern, header, len, NULL, 0, 0)) {
context->param_list.cmd = cmd;
return TRUE;
}
}
#endif
return FALSE;
}
/*
* 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
}
/**
* Reads flash address and sends it through uart
*
* Note: does not avoid the bootloader area to be read.
*
* @param address The starting address to be read
* @param size The size of memory to be read
* @return Returns the address+size (next address to be read)
*/
static inline uint16_t readFlashPage(uint16_t waddr, pagebuf_t size)
{
uint32_t address = (uint32_t)waddr<<1;
uint16_t data;
do {
#if defined(RAMPZ)
data = pgm_read_word_far(address);
#else
data = pgm_read_word_near(address);
#endif
// This is to make avrdude not complaint about
// errors when reading the interrupt table area
// after writing the flash
if(address == RESET_VECTOR_OFFSET * 2)
data = vectorTemp[0];
else if(address == PCINT0_VECTOR_OFFSET * 2)
data = vectorTemp[1];
else if(address == TIM0_COMPA_VECTOR_OFFSET * 2)
data = vectorTemp[2];
else if(address == TIM0_COMPB_VECTOR_OFFSET * 2)
data = vectorTemp[3];
swuartXmit(data); // send LSB
swuartXmit((data >> 8)); // send MSB
address += 2; // Select next word in memory
size -= 2; // Subtract two bytes from number of bytes to read
} while (size); // Repeat until block has been read
return address>>1;
}
static inline uint16_t readFlashPage(uint16_t waddr, uint8_t size)
{
uint32_t baddr = (uint32_t)waddr<<1;
uint16_t data;
uint8_t i;
i = 0x00;
do
{
#ifndef READ_PROTECT_BOOTLOADER
#warning "Bootloader not read-protected"
#if defined(RAMPZ)
data = pgm_read_word_far(baddr);
#else
data = pgm_read_word_near(baddr);
#endif
#else
// don't read bootloader
if ( baddr < APP_END )
{
#if defined(RAMPZ)
data = pgm_read_word_far(baddr);
#else
data = pgm_read_word_near(baddr);
#endif
}
else
{
data = 0xFFFF; // fake empty
}
#endif
gBuffer[i] = data; // send LSB
i++;
gBuffer[i] =data >> 8; // send MSB
i++;
baddr += 2; // Select next word in memory
size -= 2; // Subtract two bytes from number of bytes to read
}
while (size); // Repeat until block has been read
return baddr>>1;
}
bool
bootloader_is_present(void)
{
#if defined(BOOTLOADER_START_ADDRESS)
return pgm_read_word_far(BOOTLOADER_START_ADDRESS) != 0xFFFF;
#else
return false;
#endif
}
/*************************************************************************//**
*****************************************************************************/
static void *appGetIapHandler(uint32_t addr)
{
void *handler;
#if defined(__ICCAVR__)
memcpy_P((void *)&handler, (void const __farflash *)addr, sizeof(void *));
#else
handler = (void *)pgm_read_word_far(addr);
#endif
return handler;
}
uint8_t getSWversion(version_t point)
{
switch(point)
{
case version_major:
case version_minor:
case version_revision:
return (uint8_t)pgm_read_word_far(VERSION_POINT_LOCATION+(uint8_t)point*2);
default:
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);
}
}
}
bool checkNewRevision()
{
//current app crc is written to last flash location
//previous crc is stored into eeprom
//if two differ, app has changed
uint16_t crc_eeprom = eeprom_read_word((uint16_t*)CRC_LOCATION_EEPROM);
uint16_t crc_flash = pgm_read_word_far(CRC_LOCATION_FLASH);
if (crc_eeprom != crc_flash)
{
eeprom_update_word((uint16_t*)CRC_LOCATION_EEPROM, crc_flash);
return true;
}
return false;
}
static void calculateIdNumber(void){
INIT_DEBUG_PRINT("get id number\r\n");
uint32_t pgm_bytes = 0;
uint16_t crc = 0;
// calculate local id
// simply crc of user sig row
// likely to be unique and constant for each chip
NVM_CMD = NVM_CMD_READ_CALIB_ROW_gc;
uint32_t addrs[16] = {0x00,0x01,0x02,0x03,0x04,0x08,0x09,0x0A,0x0B,0x0C,0x0D,0x10,0x12,0x13,0x14,0x15};
for (uint8_t i = 0; i < 16; i++){
pgm_bytes = pgm_read_word_far(addrs[i]);
crc = _crc16_update(crc, (uint16_t)(pgm_bytes&0xFF));
}
NVM_CMD = NVM_CMD_NO_OPERATION_gc;
droplet_ID = crc;
}
void flash_write( uint32_t address, uint8_t *data, uint16_t len )
{
uint16_t page;
uint16_t offset;
uint16_t i;
uint32_t page_address;
HAS_CRITICAL_SECTION;
//if( address < ((uint32_t)(&__text_end)) || address > FLASHMEM_SIZE) {
// led_red_toggle();
// Disallow writing to kernel or bootloader
// return;
//}
ENTER_CRITICAL_SECTION();
//! making sure that EEPROM is not busy
eeprom_busy_wait ();
//
// For each different page
//
while( len > 0 ) {
page = (uint16_t)(address >> 8);
offset = (uint16_t)(address & 0x000000ff);
//
// Load data from the page into temperary buffer
//
page_address = address & 0xffffff00;
for( i = 0; i < offset; i+=2 ) {
uint16_t w = pgm_read_word_far( page_address + i );
SpmBufferFill( i, w );
}
//
// Modify the content
//
for( ; len > 0 && i < 256L; i+=2 ) {
uint16_t w = *data++;
w += (*data++) << 8;
len -= 2;
SpmBufferFill( i, w );
}
//
// load the rest of the page
//
for( ; i < 256L; i+=2 ) {
uint16_t w = pgm_read_word_far( page_address + i );
SpmBufferFill( i, w );
}
//
// Write it back
//
if( page >= 256L ) {
RAMPZ = 1;
} else {
RAMPZ = 0;
}
//
// Prepare the page address
//
page <<= 8;
SpmCommand(page, (1 << PGERS) | (1 << SPMEN));
SpmCommand(page, (1 << PGWRT) | (1 << SPMEN));
SpmCommand(0, (1 << RWWSRE) | (1 << SPMEN));
address += (256L - offset);
}
RAMPZ = 0;
LEAVE_CRITICAL_SECTION();
}
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;
}
