static void s_hal_eeprom_dspic_emuflash_write_1_page(_prog_addressT EE_address, int16_t size, void *buf)
{
int16_t buf_flash_pag1[_FLASH_PAGE]; //Note: this buffer contains 1024 bytes = num of bytes in a flash page
_prog_addressT addr_row;
int16_t *buf_row;
int16_t i;
/*
function for reading and writing from/to flash with 16 bits read and write 2 bytes for each instruction.
One instruction = 2 PC units ==> 4 bytes : 1 phantom, 3 real.
if you use function about 16 bits you use only 2 bytes of 3 real bytes
else if use function about 24 bits you use 3 bytes of 3 real bytes.
/--INSTRUCTION--/
|---|---|---|---|
| P | 1 | 2 | 3 |
|___|___|___|___|
/------//-------/
1PCunit 1PCunit
P = phantom byte
1,2,3 = real bytes used in binary code and in ...d24 flash instruction
2,3 = bytes used by ...d16 flash instructions (byte 3 is not used)
*/
/* The third parameter in this case (function ...d16) express the number
* of bytes to be written. Note that since we write 2 bytes per instruction
* (see above explanation) we are actually writing 2 bytes for each 2 PC units
* (because 1 instraction = 2 p.c. units) SO you can think about the 3rd parameter
* also as P.C. units. In other words in this case (..d16 instruction) the number
* of PC units for 3rd param is equal to the nuber of bytes.
* !!!!!!! The important thing is that this has to be checked for
* ...d24 functions!!!!!!!!
*/
_memcpy_p2d16(buf_flash_pag1, EE_address, BYTES_PER_PAGE);
_erase_flash(EE_address); //a flash page is the smallest program mem erase unit
memcpy((int8_t*)buf_flash_pag1,(int8_t*)(buf), size);
// a row is the smollest program mem program unit (a raw = 64 instructions)
addr_row = EE_address;
buf_row = buf_flash_pag1;
for(i=0; i<ROWS_PER_PAGE; i++)
{
_write_flash16(addr_row, buf_row);
addr_row += PCUNIT_PER_ROW;
buf_row += BYTES_PER_ROW/2; //divided by two because buf_row is a pointer to int16_t=2 bytes
}
}
void BootProtocolInit() {
_prog_addressT p;
bytes_out = 0;
rx_buffer_cursor = 0;
rx_message_remaining = 1;
rx_message_state = WAIT_TYPE;
ByteQueueClear(&tx_queue);
max_packet = 64;
OUTGOING_MESSAGE msg;
msg.type = ESTABLISH_CONNECTION;
msg.args.establish_connection.magic = BOOT_MAGIC;
_init_prog_address(p, hardware_version);
_memcpy_p2d16(msg.args.establish_connection.hw_impl_ver, p, 8);
_init_prog_address(p, bootloader_version);
_memcpy_p2d16(msg.args.establish_connection.bl_impl_ver, p, 8);
memcpy(msg.args.establish_connection.plat_ver, PLATFORM_ID, 8);
BootProtocolSendMessage(&msg);
}
void AppProtocolInit(CHANNEL_HANDLE h) {
_prog_addressT p;
bytes_out = 0;
rx_buffer_cursor = 0;
rx_message_remaining = 1;
rx_message_state = WAIT_TYPE;
ByteQueueClear(&tx_queue);
max_packet = ConnectionGetMaxPacket(h);
OUTGOING_MESSAGE msg;
msg.type = ESTABLISH_CONNECTION;
msg.args.establish_connection.magic = IOIO_MAGIC;
_init_prog_address(p, hardware_version);
_memcpy_p2d16(msg.args.establish_connection.hw_impl_ver, p, 8);
_init_prog_address(p, bootloader_version);
_memcpy_p2d16(msg.args.establish_connection.bl_impl_ver, p, 8);
memcpy(msg.args.establish_connection.fw_impl_ver, FW_IMPL_VER, 8);
AppProtocolSendMessage(&msg);
}
int ee_word_read(unsigned int offset)
{
#ifdef USE_EEPROM
int data = 0;
_prog_addressT p;
_init_prog_address(p, eeData);
p += offset;
_memcpy_p2d16(&data, p, 1);
return data;
#else
#warning "ee_word_read NOT using EEPROM, Flash implementation not (yet) available."
return 0;
#endif
}
extern eEbasicstrg_res_t ee_basicstrg_canaddress_get(uint32_t *canaddr)
{
if(status_notinitted == s_status)
{
eEbasicstrg_res_t res = ee_basicstrg_init();
if(ee_basicstrg_res_ok_generic == res)
{
return(ee_basicstrg_res_ok_generic);
}
}
if(status_invalid == s_status)
{
return(ee_basicstrg_res_ok_generic);
}
if(NULL == canaddr)
{
return(ee_basicstrg_res_ok_generic);
}
if(status_cached == s_status)
{
*canaddr = s_devinfo.canadr;
return(ee_basicstrg_res_ok);
}
// else .... read from flash
_memcpy_p2d16((void*)&s_devinfo, ADDR_OF_DEVICEINFO, (uint16_t)sizeof(s_deviceinfo_in_flash_t));
// now verify the crc ... use a very primitive check ...
if(0xffffffff == s_devinfo.crc)
{
memset(&s_devinfo, 0, sizeof(s_deviceinfo_in_flash_t));
s_devinfo.canadr = 1;
return(ee_basicstrg_res_ok_generic);
}
// else ...
s_status = status_cached;
*canaddr = s_devinfo.canadr;
return(ee_basicstrg_res_ok);
}
extern eEbasicstrg_res_t ee_basicstrg_init(void)
{
_memcpy_p2d16((void*)&s_beginningofloaderinfo, ADDR_OF_LOADERINFO, (uint16_t)sizeof(s_beginningofeprocinfo_in_flash_t));
if((2 != s_beginningofloaderinfo.type) || (0 != s_beginningofloaderinfo.id))
{
s_status = status_invalid;
return(ee_basicstrg_res_ok_generic);
}
s_status = status_initial;
s_devinfo.canadr = 1;
return(ee_basicstrg_res_ok);
}
unsigned char my_eeprom_read_int(Eeprom* eeprom_, unsigned long index) {
if (index >= EEPROM_30F_MAX_INDEX) {
writeError(EEPROM_OUT_OF_BOUNDS);
return -1;
}
signed int value;
_prog_addressT EE_addr;
// initialize a variable to represent the EEPROM address
_init_prog_address(EE_addr, eeData);
// read value
_memcpy_p2d16(&value, EE_addr + (index * _EE_WORD), _EE_WORD);
return (unsigned char) value;
}
/*************************************
* Initialize EEPROM
*************************************/
void InitEEPROM(void)
{
/* initialize a variable to represent the Data EEPROM address */
_init_prog_address(EE_addr, parameters_EEPROM);
_init_prog_address(EE_is_blank_addr, EE_is_blank);
/*Copy blank status flag from from DataEEPROM to RAM*/
_memcpy_p2d16(&EE_is_blank_RAM, EE_is_blank_addr, _EE_WORD);
if(EE_is_blank_RAM != 0x3FFF)
{
StoreEEPROMparams();
EE_is_blank_RAM = 0x3FFF;
_erase_eedata(EE_is_blank_addr, _EE_WORD);
_wait_eedata();
_write_eedata_word(EE_is_blank_addr, EE_is_blank_RAM);
_wait_eedata();
}
}
/*************************************
* Loads params from EEPROM to RAM
*************************************/
void LoadEEPROMparams(void)
{
/*Copy array "parameters_EEPROM" from DataEEPROM to "parameters_RAM" in RAM*/
_memcpy_p2d16(parameters_RAM, EE_addr, _EE_ROW);
_memcpy_p2d16(parameters_RAM+16, EE_addr+32, _EE_ROW);
}
void RecoverConfigurationFromEEprom()
{
_prog_addressT EE_addr;
_init_prog_address(EE_addr, ee_data);
_memcpy_p2d16(&BoardConfig, EE_addr, sizeof(BoardConfig));
}
static int s_canIcubProtoParser_parse_pollingMsg(tCanData *rxpayload, unsigned char rxlen, tCanData *txpayload, unsigned char *txlen)
{
unsigned char cmd = rxpayload->b[0];
*txlen=0;
switch (cmd)
{
case ICUBCANPROTO_POL_MC_CMD__CONTROLLER_RUN: // DS402 Operation Enable
{
if (rxlen!=1 || !DS402_Statusword.Flags.SwitchedOn || !sCanProtocolCompatible) return 0;
//when start PWM ==> set the default control mode
SysStatus.b[0] = 0;
// go to Operation Enable state
DS402_Controlword.Flags.EnableOperation = 1;
}
break;
case ICUBCANPROTO_POL_MC_CMD__DISABLE_PWM_PAD: // DS402 Operation Disable
{
if (rxlen!=1) return 0;
FaultReset();
// Command can be accepted only if current ststus is OPERATION ENABLE
if (!DS402_Statusword.Flags.OperationEnabled && !DS402_Statusword.Flags.SwitchedOn) return 0;
// In ICUB the states DISABLE_OPERATION and SHUTDOWN goes in the same state.
// go to Ready to Switch On state
DS402_Controlword.Flags.EnableVoltage = 0;
}
break;
case ICUBCANPROTO_POL_MC_CMD__ENABLE_PWM_PAD: // DS402 Switch On
{
if (rxlen!=1 || !DS402_Statusword.Flags.ReadyToSwitchOn || !sCanProtocolCompatible) return 0;
received_canloader_msg = 0; // start to transmit status messages
// go to Switch On state
DS402_Controlword.Flags.SwitchOn = 1;
}
break;
case ICUBCANPROTO_POL_MC_CMD__CONTROLLER_IDLE: // DS402 Shutdown
{
if (rxlen!=1 || (!DS402_Statusword.Flags.OperationEnabled && !DS402_Statusword.Flags.SwitchedOn)) return 0;
// go to Ready to Switch On state
//DS402_Controlword.Flags.EnableVoltage = 0;
DS402_Controlword.Flags.EnableOperation = 0;
}
break;
case ICUBCANPROTO_POL_MC_CMD__GET_CONTROL_MODE:
{
if (rxlen!=1) return 0;
txpayload->b[1] = CanIcubProtoGetcontrol_mode();
if (txpayload->b[1] == icubCanProto_controlmode_unknownError) return 0;
*txlen = 2;
txpayload->b[0] = cmd;
}
break;
case ICUBCANPROTO_POL_MC_CMD__SET_CONTROL_MODE:
{
if (rxlen!=2 || !sCanProtocolCompatible) return 0;
received_canloader_msg = 0; // start to transmit status messages
iCubProtControlMode = rxpayload->b[1];
CtrlReferences.IqRef = 0;
CtrlReferences.VqRef = 0;
CtrlReferences.WRef = 0;
if (rxpayload->b[1] != icubCanProto_controlmode_idle)
{
DS402_Controlword.Flags.SwitchOn = 1;
DS402_Controlword.Flags.EnableOperation = 1;
}
switch (rxpayload->b[1])
{
case icubCanProto_controlmode_openloop:
SysStatus.b[0] = 0;
break;
case icubCanProto_controlmode_current:
SysStatus.b[0] = 1;
break;
case icubCanProto_controlmode_velocity:
case icubCanProto_controlmode_speed_voltage:
SysStatus.b[0] = 2;
break;
case icubCanProto_controlmode_speed_current:
SysStatus.b[0] = 3;
break;
case icubCanProto_controlmode_idle:
FaultReset();
if (!Fault()) LED_status.GreenBlinkRate = BLINKRATE_FAST;
DS402_Controlword.Flags.EnableOperation = 0; // go to Switched On state
}
}
break;
case ICUBCANPROTO_POL_MC_CMD__WRITE_FLASH_MEM:
//EepromSave();
break;
case ICUBCANPROTO_POL_MC_CMD__GET_ADDITIONAL_INFO:
case ICUBCANPROTO_POL_MC_CMD__SET_ADDITIONAL_INFO:
case ICUBCANPROTO_POL_MC_CMD__GET_DESIRED_VELOCITY:
case ICUBCANPROTO_POL_MC_CMD__SET_DESIRED_VELOCITY:
break;
case ICUBCANPROTO_POL_MC_CMD__SET_ENCODER_POSITION:
{
// setta l'offset di fasatura
//Encoder_SyncPulsePosition = (rxpayload->b[2] << 8 | rxpayload->b[1]); //todo aspetta maggia
}
break;
case ICUBCANPROTO_POL_MC_CMD__GET_BOARD_ID:
{
if (rxlen!=1) return 0;
*txlen=2;
txpayload->b[0] = cmd;
txpayload->b[1] = canprotoparser_bid;
}
break;
case ICUBCANPROTO_POL_MC_CMD__SET_BOARD_ID:
{
if (rxlen!=2) return 0;
#ifndef CAN_CMD_ALWAYS_ACCEPT
if (!DS402_Statusword.Flags.OperationEnabled && !DS402_Statusword.Flags.SwitchedOn) return 0;
#endif
canprotoparser_bid = rxpayload->b[1];
ApplicationData.EepromHeader.EE_CAN_BoardAddress = canprotoparser_bid;
CanIcubProtoTransmitterUpdateBoardId(canprotoparser_bid);
CanIcubProtoSetFilters(canprotoparser_bid);
_memcpy_p2d16(&s_devinfo, 0x15000, sizeof(s_deviceinfo_in_flash_t));
_erase_flash(0x15000);
s_devinfo.canadr = canprotoparser_bid;
s_devinfo.mode = 1;
s_devinfo.dummy0 = 0;
s_devinfo.dummy1 = 0xcaac;
s_devinfo.crc = crc16(0xFFFF, (const unsigned char*)&s_devinfo, 64);
int i;
for (i=0; i<sizeof(s_deviceinfo_in_flash_t); i+=_FLASH_ROW)
{
_write_flash16(0x15000+i, ((int*)(&s_devinfo))+i/2);
}
}
break;
case ICUBCANPROTO_POL_MC_CMD__SET_MAX_VELOCITY:
case ICUBCANPROTO_POL_MC_CMD__GET_MAX_VELOCITY:
break;
case ICUBCANPROTO_POL_MC_CMD__SET_CURRENT_LIMIT:
{
if (rxlen!=5) return 0;
////il dato che arriva è espresso in mA va trasformato in IAD della 2FOC (1A 1310)
//il dato che arriva è espresso in mA va trasformato in IAD della 2FOC (1A=2000)
ApplicationData.CurLimit = (rxpayload->b[2] << 8 | rxpayload->b[1])*2;
}
break;
case ICUBCANPROTO_POL_MC_CMD__GET_FIRMWARE_VERSION:
{
uint8_t server_can_protocol_major = rxpayload->b[1];
uint8_t server_can_protocol_minor = rxpayload->b[2];
sCanProtocolCompatible = (
CAN_PROTOCOL_VERSION_MAJOR == server_can_protocol_major &&
CAN_PROTOCOL_VERSION_MINOR == server_can_protocol_minor);
*txlen = 0x8;
txpayload->b[0] = ICUBCANPROTO_POL_MC_CMD__GET_FIRMWARE_VERSION;
txpayload->b[1] = icubCanProto_boardType__2foc;
txpayload->b[2] = FW_VERSION_MAJOR;
txpayload->b[3] = FW_VERSION_MINOR;
txpayload->b[4] = FW_VERSION_BUILD;
txpayload->b[5] = CAN_PROTOCOL_VERSION_MAJOR;
txpayload->b[6] = CAN_PROTOCOL_VERSION_MINOR;
txpayload->b[7] = sCanProtocolCompatible; // can_protocol_ack;
if (!sCanProtocolCompatible)
{
// go to fault state
SysError.CANInvalidProtocol = 1;
// call fault handler
FaultConditionsHandler();
}
}
break;
case ICUBCANPROTO_POL_MC_CMD__SET_CURRENT_PID:
{
SFRAC16 pp, pi, pd, pm; //pm non viene passato dal comando
if (rxlen!=7) return 0;
//ControllerGetCurrentDPIDParm(&pp,&pi,&pd,&pm);
pp = (rxpayload->b[2] << 8 | rxpayload->b[1]);
pi = (rxpayload->b[4] << 8 | rxpayload->b[3]);
pd = 0;
pm = (rxpayload->b[6] << 8 | rxpayload->b[5]);
//ControllerSetCurrentDPIDParm(pp, pi, pd, pm);
//ControllerSetCurrentQPIDParm(pp, pi, pd, pm);
}
break;
case ICUBCANPROTO_POL_MC_CMD__GET_CURRENT_PID:
{
signed int p, i, d;
if (rxlen!=1) return 0;
//ControllerGetCurrentDPIDParm(&p, &i, &d, &m);
txpayload->b[0] = cmd;
memcpy(&txpayload->b[1], &p, 2);
memcpy(&txpayload->b[3], &i, 2);
memcpy(&txpayload->b[5], &d, 2);
*txlen = 7;
}
break;
case ICUBCANPROTO_POL_MC_CMD__SET_VELOCITY_PID:
{
SFRAC16 pp, pi, pd; //pm non viene passato dal comando
if (rxlen!=7) return 0;
//ControllerGetWPIDParm(&pp, &pi, &pd, &pm);
pp = (rxpayload->b[2] << 8 | rxpayload->b[1]);
pi = (rxpayload->b[4] << 8 | rxpayload->b[3]);
pd = (rxpayload->b[6] << 8 | rxpayload->b[5]);
//ControllerSetWPIDParm(pp, pi, pd, pm);
}
break;
case ICUBCANPROTO_POL_MC_CMD__GET_VELOCITY_PID:
{
signed int p, i, d;
if (1 != rxlen) return (0);
//ControllerGetWPIDParm(&p, &i, &d, &m);
txpayload->b[0] = cmd;
memcpy(&txpayload->b[1], &p, 2);
memcpy(&txpayload->b[3], &i, 2);
memcpy(&txpayload->b[5], &d, 2);
*txlen = 7;
}
break;
case ICUBCANPROTO_POL_MC_CMD__SET_DESIRED_CURRENT:
{
if (rxlen!=5) return 0;
#ifndef CAN_CMD_ALWAYS_ACCEPT
if (!DS402_Statusword.Flags.OperationEnabled && !DS402_Statusword.Flags.SwitchedOn) return 0;
#endif
// Torque control references
int ctrlRef = (rxpayload->b[2] << 8 | rxpayload->b[1]);
switch (iCubProtControlMode)
{
case icubCanProto_controlmode_velocity:
case icubCanProto_controlmode_speed_voltage:
case icubCanProto_controlmode_speed_current:
CtrlReferences.WRef = ctrlRef;
LIMIT(CtrlReferences.WRef, UDEF_SPEED_MAX)
break;
case icubCanProto_controlmode_current:
CtrlReferences.IqRef = ctrlRef;
LIMIT(CtrlReferences.IqRef, UDEF_CURRENT_MAX)
break;
case icubCanProto_controlmode_openloop:
CtrlReferences.VqRef = ctrlRef;
LIMIT(CtrlReferences.VqRef, UDEF_PWM_MAX)
break;
case icubCanProto_controlmode_idle:
break;
}
#ifdef SYNC_2FOC_TO_EMS
CanIcubProtoTrasmitterSendPeriodicData();
#endif
}
break;
case ICUBCANPROTO_POL_MC_CMD__GET_DESIRED_CURRENT:
{
if (rxlen!=1) return 0;
*txlen = 5;
txpayload->b[0] = cmd;
memcpy(&txpayload->b[1], &CtrlReferences.IqRef, 2);
txpayload->b[3] = txpayload->b[4] = 0;
}
break;
case ICUBCANPROTO_POL_MC_CMD__SET_PERIODIC_MSG_CONTENTS:
{
if (rxlen!=5) return 0;
// check data to transmit if in the acceptable range
if ((rxpayload->b[1] < ELEMENTS_IN_PREIODIC_DATA_LIST)
&& (rxpayload->b[2] < ELEMENTS_IN_PREIODIC_DATA_LIST)
&& (rxpayload->b[3] < ELEMENTS_IN_PREIODIC_DATA_LIST)
&& (rxpayload->b[4] < ELEMENTS_IN_PREIODIC_DATA_LIST))
{
// set the data to transmit
PeriodicMessageContents[0] = rxpayload->b[1];
PeriodicMessageContents[1] = rxpayload->b[2];
PeriodicMessageContents[2] = rxpayload->b[3];
PeriodicMessageContents[3] = rxpayload->b[4];
// precalculate this here. This is an optimization
gulpadr1 = (unsigned int*) PeriodicData[PeriodicMessageContents[0]];
gulpadr2 = (unsigned int*) PeriodicData[PeriodicMessageContents[1]];
gulpadr3 = (unsigned int*) PeriodicData[PeriodicMessageContents[2]];
gulpadr4 = (unsigned int*) PeriodicData[PeriodicMessageContents[3]];
}
else
{
return 0;
}
}
break;
case ICUBCANPROTO_POL_MC_CMD__SET_I2T_PARAMS:
{
if (rxlen!=5) return 0;
//extract params from the 1st word
I2Tdata.Param = (rxpayload->b[2] << 8 | rxpayload->b[1]);
I2Tdata.IThreshold = (rxpayload->b[4] << 8 | rxpayload->b[3]);
}
break;
case ICUBCANPROTO_POL_MC_CMD__GET_I2T_PARAMS:
{
if (rxlen!=1) return 0;
*txlen = 5;
txpayload->b[0] = cmd;
memcpy(&txpayload->b[1], &I2Tdata.Param, 2);
memcpy(&txpayload->b[3], &I2Tdata.IThreshold, 2);
}
break;
default:
return 0;
} //end switch
return 1;
}
static void s_hal_eeprom_dspic_emuflash_read(void *data, _prog_addressT addr, int16_t size)
{
#warning -> acemor: maybe a check upon validity of the address and size ??
_memcpy_p2d16(data, addr, size);
}
