void DCLinkCheck()
// verify Under and Over voltages conditions
{
// calc vdc mean with previous sample in order to filter a bit
vdc += ADCVDCLinkTo100mV(VDCLink);
vdc /= 2;
// TODO: questa roba qui dentro non ci azzecca una favonia.
// undervoltage check
// TODO: portare le variabili in ram come per l'I2T
if(vdc < UNDERVOLTAGE_THRESHOLD)
{
// TODO: passare al FaultConditionsHandler il codice del fault
// e al suo interno settare il relativo bit nello stato
// Set Undervoltage flag
SysError.UnderVoltageFailure = 1;
FaultConditionsHandler();
}
// overvoltage check
if(vdc > OVERVOLTAGE_THRESHOLD)
{
// TODO: passare al FaultConditionsHandler il codice del fault
// e al suo interno settare il relativo bit nello stato
// TODO: verificare che tutti i fault (overcurrent...) settino l'apposito flag nello stato
// Set overvoltage flag
SysError.OverVoltageFailure = 1;
FaultConditionsHandler();
}
}
void FaultRecheck()
// retrigger the fault handler if
// a fault happened.
// This is used if a fault happened but someone might
// have enabled PWM or CAN or changed LEDs to restore
// the fault state
{
if(fault)
{
fault = 0;
FaultConditionsHandler();
}
}
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;
}
