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