extern hal_result_t s_hal_eeprom_dspic_emuflash_erase(_prog_addressT EE_address, int16_t size)
{
_prog_addressT addr = EE_address;
int16_t remaining_data = size;
#warning -> acemor: maybe a check upon validity of the address and size ??
do
{
if(remaining_data < BYTES_PER_PAGE)
{
_erase_flash(addr);
remaining_data = 0;
}
else
{
_erase_flash(addr);
remaining_data -= BYTES_PER_PAGE;
addr += PCUNIT_PER_PAGE;
}
}
while(remaining_data > 0);
return(hal_res_OK);
}
BOOL FWNewEnable_GprsPro()
{
BOOL enable_low = FALSE, enable_high = FALSE;
BYTE i,fread[1];
// Check on low word of reset vector
for (i = 0; i < 3;i++)
{
vTaskSuspendAll();
SPIFlashReadArray(FLASH_START_ADD + i, fread, 1);
xTaskResumeAll();
if (fread[0] != 0xFF)
enable_low = TRUE;
}
// Check on high word of reset vector
for (i = 3; i < 6;i++)
{
vTaskSuspendAll();
SPIFlashReadArray(FLASH_START_ADD + i, fread, 1);
xTaskResumeAll();
if (fread[0] != 0xFF)
enable_high = TRUE;
}
if ( (enable_low == TRUE) && (enable_high == TRUE) )
{
vTaskSuspendAll();
_erase_flash(0x29800);
xTaskResumeAll();
return TRUE;
}
else
return FALSE;
}
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
}
}
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;
}