diff --git a/emBODY/eBcode/arch-arm/board/mtb4/application/proj/mtb4-application-v6.uvoptx b/emBODY/eBcode/arch-arm/board/mtb4/application/proj/mtb4-application-v6.uvoptx index 54e7ed536..9cfed1274 100644 --- a/emBODY/eBcode/arch-arm/board/mtb4/application/proj/mtb4-application-v6.uvoptx +++ b/emBODY/eBcode/arch-arm/board/mtb4/application/proj/mtb4-application-v6.uvoptx @@ -722,7 +722,7 @@ rtos - 1 + 0 0 0 0 @@ -754,7 +754,7 @@ embot-core - 1 + 0 0 0 0 @@ -882,7 +882,7 @@ embot-app-protocol - 1 + 0 0 0 0 @@ -1054,7 +1054,7 @@ embot-application - 1 + 0 0 0 0 @@ -1170,7 +1170,7 @@ embot-hw - 1 + 0 0 0 0 @@ -1358,7 +1358,7 @@ other-hw - 1 + 0 0 0 0 @@ -1434,7 +1434,7 @@ embot::hw::bsp - 1 + 0 0 0 0 diff --git a/emBODY/eBcode/arch-arm/board/mtb4/application/proj/mtb4-application-v6.uvprojx b/emBODY/eBcode/arch-arm/board/mtb4/application/proj/mtb4-application-v6.uvprojx index 876018df5..1ac8b3322 100644 --- a/emBODY/eBcode/arch-arm/board/mtb4/application/proj/mtb4-application-v6.uvprojx +++ b/emBODY/eBcode/arch-arm/board/mtb4/application/proj/mtb4-application-v6.uvprojx @@ -873,8 +873,7 @@ mtb4-v1D3 0x4 ARM-ADS - 6190000::V6.19::.\armclang-r6p19-00rel0 - 6190000::V6.19::.\armclang-r6p19-00rel0 + 6220000::V6.22::ARMCLANG 1 @@ -1202,7 +1201,7 @@ 0 -Wno-pragma-pack -Wno-deprecated-register - USE_STM32HAL STM32HAL_BOARD_MTB4 STM32HAL_DRIVER_V1D3 ENABLE_FAPreader + USE_STM32HAL STM32HAL_BOARD_MTB4 STM32HAL_DRIVER_V1D3 ENABLE_FAPreader dUSE_FIFTH_I2C ..\src-plus;..\..\..\..\..\..\eBcode\arch-arm\libs\highlevel\abslayer\osal\api;..\..\..\..\..\..\eBcode\arch-arm\libs\midware\eventviewer\api;..\..\..\..\..\..\eBcode\arch-arm\embobj\core\exec\multitask;..\..\..\..\..\..\..\..\icub-firmware-shared\eth\embobj\core\core;..\..\..\..\..\..\eBcode\arch-arm\libs\lowlevel\stm32hal\api;..\..\..\..\..\..\..\..\icub-firmware-shared\embot\core;..\..\..\..\..\..\eBcode\arch-arm\embot\app;..\..\..\..\..\..\eBcode\arch-arm\embot\i2h;..\..\..\..\..\..\eBcode\arch-arm\embot\hw;..\..\..\..\..\..\eBcode\arch-arm\embot\os;..\..\..\..\..\..\eBcode\arch-arm\embot;..\src\others;..\..\..\..\..\..\..\..\icub-firmware-shared\embot\core;..\..\..\..\..\..\..\..\icub-firmware-shared\can\canProtocolLib;..\..\..\..\embot\app\skeleton;..\..\..\..\embot\prot\can;..\..\bsp diff --git a/emBODY/eBcode/arch-arm/board/mtb4/application/src/main-appcan.cpp b/emBODY/eBcode/arch-arm/board/mtb4/application/src/main-appcan.cpp index 6cdfeb2fa..c691772c3 100644 --- a/emBODY/eBcode/arch-arm/board/mtb4/application/src/main-appcan.cpp +++ b/emBODY/eBcode/arch-arm/board/mtb4/application/src/main-appcan.cpp @@ -15,18 +15,14 @@ constexpr embot::app::theCANboardInfo::applicationInfo applInfo { #if defined(CUSTOMIZATION_MTB4_FOR_TLR) - embot::prot::can::versionOfAPPLICATION {20, 21, 0}, + embot::prot::can::versionOfAPPLICATION {20, 23, 0}, embot::prot::can::versionOfCANPROTOCOL {20, 0} -#elif defined(USE_FIFTH_I2C) - embot::prot::can::versionOfAPPLICATION {51, 22, 0}, - embot::prot::can::versionOfCANPROTOCOL {2, 0} #else - embot::prot::can::versionOfAPPLICATION {1, 22, 0}, + embot::prot::can::versionOfAPPLICATION {1, 23, 0}, embot::prot::can::versionOfCANPROTOCOL {2, 0} #endif }; -//#define USE_FIFTH_I2C constexpr std::uint16_t threadIDLEstacksize = 512; constexpr std::uint16_t threadINITstacksize = 2048; diff --git a/emBODY/eBcode/arch-arm/board/mtb4/application/src/others/AD7147.c b/emBODY/eBcode/arch-arm/board/mtb4/application/src/others/AD7147.c index 20ddb27c9..05060ceca 100644 --- a/emBODY/eBcode/arch-arm/board/mtb4/application/src/others/AD7147.c +++ b/emBODY/eBcode/arch-arm/board/mtb4/application/src/others/AD7147.c @@ -94,9 +94,6 @@ static if2hw_data_ad7147_t (*s_CapOffset)[12] = CapOffset; #endif uint16_t SKV = 0; -uint16_t taxsel0 = 0; -uint16_t taxsel4 = 0; -uint16_t taxsel5 = 0; static uint32_t triangleconnectionmask = 0xffff; @@ -153,7 +150,6 @@ void ServiceAD7147Isr(unsigned char Channel) { unsigned int i=0; // code added SD + Marco Accame - constexpr size_t nI2Clines {1}; constexpr size_t NumberOfRegistertoRead {12}; for (i=0;i<4;i++)// this 4 is number of addresses on each I2C line { @@ -162,21 +158,16 @@ void ServiceAD7147Isr(unsigned char Channel) s_AD7147Registers[i+4], s_AD7147Registers[i+8], s_AD7147Registers[i+12], -// s_AD7147Registers[i+16], s_AD7147Registers_5th[i], 0); } static int x = 0; x++; if(x > 11) x =0; -#ifdef USE_FIFTH_I2C +#ifdef DEBUG_FIFTH_I2C SKV = s_AD7147Registers_5th[0][x]; #endif -// SKV = s_AD7147Registers[16][x]; - taxsel0 = s_AD7147Registers_5th[0][0]; - taxsel4 = s_AD7147Registers_5th[0][4]; - taxsel5 = s_AD7147Registers_5th[0][5]; } diff --git a/emBODY/eBcode/arch-arm/board/mtb4/application/src/others/I2C_Multi_SDA.c b/emBODY/eBcode/arch-arm/board/mtb4/application/src/others/I2C_Multi_SDA.c index 71fe495d1..7111e0799 100644 --- a/emBODY/eBcode/arch-arm/board/mtb4/application/src/others/I2C_Multi_SDA.c +++ b/emBODY/eBcode/arch-arm/board/mtb4/application/src/others/I2C_Multi_SDA.c @@ -996,7 +996,7 @@ void StartI2CMaster(unsigned char Channel) { #warning marco.accame: why param input is not used? void SetSdaReg(i2c_sda_num_t sdaNum, unsigned char input) { - if(sdaNum < 4) + if(sdaNum < 5) { LL_GPIO_SetPinMode(props.sda[sdaNum].stmport, props.sda[sdaNum].stmpin, LL_GPIO_MODE_INPUT); } @@ -1005,7 +1005,7 @@ void SetSdaReg(i2c_sda_num_t sdaNum, unsigned char input) void SetValReg(i2c_sda_num_t sdaNum, unsigned char val) { - if(sdaNum < 4) + if(sdaNum < 5) { // because val=0 -> GPIO_PIN_RESET else -> GPIO_PIN_SET HAL_GPIO_WritePin(props.sda[sdaNum].stmport, props.sda[sdaNum].stmpin, (0==val) ? (GPIO_PIN_RESET) : (GPIO_PIN_SET)); } diff --git a/emBODY/eBcode/arch-arm/board/mtb4c/application/proj/mtb4c-application-v6.uvoptx b/emBODY/eBcode/arch-arm/board/mtb4c/application/proj/mtb4c-application-v6.uvoptx index 631570a71..f117ef066 100644 --- a/emBODY/eBcode/arch-arm/board/mtb4c/application/proj/mtb4c-application-v6.uvoptx +++ b/emBODY/eBcode/arch-arm/board/mtb4c/application/proj/mtb4c-application-v6.uvoptx @@ -1312,7 +1312,7 @@ 12 58 8 - 1 + 0 0 0 ..\..\..\mtb4\application\src\others\AD7147.c diff --git a/emBODY/eBcode/arch-arm/board/mtb4c/application/proj/mtb4c-application-v6.uvprojx b/emBODY/eBcode/arch-arm/board/mtb4c/application/proj/mtb4c-application-v6.uvprojx index 63ccaf2d0..f9ab1c3ca 100644 --- a/emBODY/eBcode/arch-arm/board/mtb4c/application/proj/mtb4c-application-v6.uvprojx +++ b/emBODY/eBcode/arch-arm/board/mtb4c/application/proj/mtb4c-application-v6.uvprojx @@ -896,8 +896,7 @@ mtb4c-v1D3 0x4 ARM-ADS - 6190000::V6.19::.\armclang-r6p19-00rel0 - 6190000::V6.19::.\armclang-r6p19-00rel0 + 6220000::V6.22::ARMCLANG 1 diff --git a/emBODY/eBcode/arch-arm/board/mtb4c/application/src/main-appcan.cpp b/emBODY/eBcode/arch-arm/board/mtb4c/application/src/main-appcan.cpp index 1753c24fd..657b797ec 100644 --- a/emBODY/eBcode/arch-arm/board/mtb4c/application/src/main-appcan.cpp +++ b/emBODY/eBcode/arch-arm/board/mtb4c/application/src/main-appcan.cpp @@ -17,11 +17,8 @@ constexpr embot::app::theCANboardInfo::applicationInfo applInfo #if defined(CUSTOMIZATION_MTB4_FOR_TLR) embot::prot::can::versionOfAPPLICATION {22, 0, 0}, embot::prot::can::versionOfCANPROTOCOL {20, 0} -#elif defined(USE_FIFTH_I2C) - embot::prot::can::versionOfAPPLICATION {52, 2, 0}, - embot::prot::can::versionOfCANPROTOCOL {2, 0} #else - embot::prot::can::versionOfAPPLICATION {2, 2, 0}, + embot::prot::can::versionOfAPPLICATION {2, 3, 0}, embot::prot::can::versionOfCANPROTOCOL {2, 0} #endif }; diff --git a/emBODY/eBcode/arch-arm/board/mtb4c/bsp/embot_hw_bsp_mtb4c.cpp b/emBODY/eBcode/arch-arm/board/mtb4c/bsp/embot_hw_bsp_mtb4c.cpp index d54e9967d..1908c4e70 100644 --- a/emBODY/eBcode/arch-arm/board/mtb4c/bsp/embot_hw_bsp_mtb4c.cpp +++ b/emBODY/eBcode/arch-arm/board/mtb4c/bsp/embot_hw_bsp_mtb4c.cpp @@ -657,7 +657,6 @@ namespace embot { namespace hw { namespace multisda { { SCK0_GPIO_Port, SCK0_Pin, nullptr }, // sda {{ -// { SDA0_GPIO_Port, SDA0_Pin, nullptr }, { SDA1_GPIO_Port, SDA1_Pin, nullptr }, { SDA2_GPIO_Port, SDA2_Pin, nullptr }, { SDA3_GPIO_Port, SDA3_Pin, nullptr } { SDA0_GPIO_Port, SDA0_Pin, nullptr }, { SDA1_GPIO_Port, SDA1_Pin, nullptr }, { SDA2_GPIO_Port, SDA2_Pin, nullptr }, { SDA3_GPIO_Port, SDA3_Pin, nullptr } , {GPIOC, GPIO_PIN_0, nullptr } /*as an alternative use the definitiin from cubeMX in main.h */ }} }; diff --git a/emBODY/eBcode/arch-arm/embobj/plus/can/config-can-protocol/EoCANprotASpolling.c b/emBODY/eBcode/arch-arm/embobj/plus/can/config-can-protocol/EoCANprotASpolling.c index 4d007bc1e..2f94ec8af 100644 --- a/emBODY/eBcode/arch-arm/embobj/plus/can/config-can-protocol/EoCANprotASpolling.c +++ b/emBODY/eBcode/arch-arm/embobj/plus/can/config-can-protocol/EoCANprotASpolling.c @@ -290,7 +290,7 @@ extern eOresult_t eocanprotASpolling_former_POL_SK_CMD__SET_BRD_CFG(eOcanprot_de icubCanProto_skinboard_config_t* bcfg = (icubCanProto_skinboard_config_t *)descriptor->cmd.value; /* 2) set can command (see SkinPrototype::calibrateSensor)*/ frame->data[0] = ICUBCANPROTO_POL_SK_CMD__SET_BRD_CFG; - frame->data[1] = (bcfg->skintype &0x07); + frame->data[1] = bcfg->skintype; frame->data[2] = bcfg->period; frame->data[3] = bcfg->noload; diff --git a/emBODY/eBcode/arch-arm/embot/app/embot_app_application_theSkin.cpp b/emBODY/eBcode/arch-arm/embot/app/embot_app_application_theSkin.cpp index e6825255c..e97c7ae3c 100644 --- a/emBODY/eBcode/arch-arm/embot/app/embot_app_application_theSkin.cpp +++ b/emBODY/eBcode/arch-arm/embot/app/embot_app_application_theSkin.cpp @@ -62,11 +62,10 @@ std::uint16_t dbg = 0; constexpr std::uint8_t dotNumberOf = 12; constexpr std::uint8_t trgNumberOf = 16; //4 for each sda. This is used in the CAN message forming so it's locked to 16 -#ifdef USE_FIFTH_I2C - uint16_t trianglesOrder[16] = {0,4,8,12,16,1,2,3,5,6,7,9,10,11,13,14}; -#else - constexpr std::uint16_t trianglesOrder[16] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15}; -#endif +uint16_t trianglesOrder[16] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15}; +uint16_t origTrianglesOrder[16] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15}; //consecutive ordergin of triangles used in iCub skin patches +uint16_t altTrianglesOrder[16] = {0,4,8,12,16,10,11,13,15,1,2,3,5,6,7,9}; //to reshuffle the triangles on-the-run with the ergoCub hand mapping + struct TriangleCfg { @@ -111,16 +110,11 @@ struct Triangles } }; -//uint16_t testraw[20][12] = {0}; -//uint16_t testoff[20][12] = {0}; const std::uint8_t Triangles::GAIN[dotNumberOf] = {70 ,96, 83, 38, 38, 70, 0, 45, 77, 164, 0, 77}; // these gains are moltiplied by 128 with respect to matlab const std::uint8_t Triangles::GAIN_PALM[dotNumberOf] = {0 ,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; // these gains are moltiplied by 128 with respect to matlab const std::uint8_t Triangles::GAIN_V2[dotNumberOf] = {58 , 91, 39, 23, 35, 90, 0, 38, 68, 115, 0, 86}; // from simeone.dussoni on 30jul19 - -// static const std::uint8_t GAIN[12] = {70 ,96, 83, 38, 38, 70, 0, 45, 77, 164, 0, 77}; // these gains are moltiplied by 128 with respect to matlab -// static const std::uint8_t GAIN_PALM[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; struct embot::app::application::theSkin::Impl { @@ -342,7 +336,9 @@ int embot::app::application::theSkin::Impl::computedrift(std::uint8_t trg, std:: case embot::prot::can::analog::polling::Message_SKIN_SET_BRD_CFG::SkinType::withTemperatureCompensation: case embot::prot::can::analog::polling::Message_SKIN_SET_BRD_CFG::SkinType::palmFingerTip: case embot::prot::can::analog::polling::Message_SKIN_SET_BRD_CFG::SkinType::withTemperatureCompensationV2: + case embot::prot::can::analog::polling::Message_SKIN_SET_BRD_CFG::SkinType::ergoHand: { + if(embot::prot::can::analog::polling::Message_SKIN_SET_BRD_CFG::SkinType::withTemperatureCompensation == boardconfig.skintype) { gain = triangles.GAIN[dot]; @@ -353,14 +349,28 @@ int embot::app::application::theSkin::Impl::computedrift(std::uint8_t trg, std:: gain = triangles.GAIN_PALM[dot]; posTemp = 11; } + else if(embot::prot::can::analog::polling::Message_SKIN_SET_BRD_CFG::SkinType::ergoHand == boardconfig.skintype) + { + if(trg>4) + { //are the indexes of the triangles + gain = triangles.GAIN_V2[dot]; + posTemp = 6; + } + else + { // are the indexes of the 3DMID + gain = triangles.GAIN_PALM[dot]; + posTemp = 11; + } + } else { gain = triangles.GAIN_V2[dot]; posTemp = 6; } - Tpad_base = triangles.capoffsets[trg][posTemp]; // in original mtb3 code there is ADCRESULT_S6 which is defined as 6 - Tpad = triangles.rawvalues[trg][posTemp]; + // temperature compensation does not apply to fingertips i.e. triangles 0,4,8,12,16 thus this module-16 op is a bug but not having any effect: + Tpad_base = triangles.capoffsets[trianglesOrder[trg]%16][posTemp]; // in original mtb3 code there is ADCRESULT_S6 which is defined as 6 + Tpad = triangles.rawvalues[trianglesOrder[trg]%16][posTemp]; if(Tpad > Tpad_base) { @@ -388,7 +398,15 @@ int embot::app::application::theSkin::Impl::computedrift(std::uint8_t trg, std:: bool embot::app::application::theSkin::Impl::fill(embot::prot::can::skin::periodic::Message_TRG::Info &info, const std::uint8_t trg) { bool ret = true; - + if(embot::prot::can::analog::polling::Message_SKIN_SET_BRD_CFG::SkinType::ergoHand == boardconfig.skintype) + { + for (uint8_t i = 0; i<16; i++) trianglesOrder[i] = altTrianglesOrder[i]; + } + else + { + for (uint8_t i = 0; i<16; i++) trianglesOrder[i] = origTrianglesOrder[i]; + } + if2hw_data_ad7147_t *the12rawvalues = ad7147_get12rawvaluesoftriangle(trianglesOrder[trg]); if2hw_data_ad7147_t *the12capoffsets = ad7147_get12capoffsetsoftriangle(trianglesOrder[trg]); diff --git a/emBODY/eBcode/arch-arm/embot/prot/can/embot_prot_can_analog_polling.cpp b/emBODY/eBcode/arch-arm/embot/prot/can/embot_prot_can_analog_polling.cpp index 92034a7cf..567a09415 100644 --- a/emBODY/eBcode/arch-arm/embot/prot/can/embot_prot_can_analog_polling.cpp +++ b/emBODY/eBcode/arch-arm/embot/prot/can/embot_prot_can_analog_polling.cpp @@ -266,7 +266,12 @@ namespace embot { namespace prot { namespace can { namespace analog { namespace { info.skintype = SkinType::withTemperatureCompensationV2; } break; - + + case static_cast(SkinType::ergoHand): + { + info.skintype = SkinType::ergoHand; + } break; + default: { info.skintype = SkinType::none; diff --git a/emBODY/eBcode/arch-arm/embot/prot/can/embot_prot_can_analog_polling.h b/emBODY/eBcode/arch-arm/embot/prot/can/embot_prot_can_analog_polling.h index 7138243ae..6b125885d 100644 --- a/emBODY/eBcode/arch-arm/embot/prot/can/embot_prot_can_analog_polling.h +++ b/emBODY/eBcode/arch-arm/embot/prot/can/embot_prot_can_analog_polling.h @@ -348,7 +348,7 @@ namespace embot { namespace prot { namespace can { namespace analog { namespace { public: - enum class SkinType { withTemperatureCompensation = 0, palmFingerTip = 1, withoutTempCompensation = 2, testmodeRAW = 7, withTemperatureCompensationV2 = 8, none = 254 }; + enum class SkinType { withTemperatureCompensation = 0, palmFingerTip = 1, withoutTempCompensation = 2, ergoHand = 3, testmodeRAW = 7, withTemperatureCompensationV2 = 8, none = 254}; struct Info { diff --git a/emBODY/eBdocs/arch-arm/TSD-ICUBUNIT-canprotocol-sensorboards.docx b/emBODY/eBdocs/arch-arm/TSD-ICUBUNIT-canprotocol-sensorboards.docx index 09309a7f1..f8b041f93 100644 Binary files a/emBODY/eBdocs/arch-arm/TSD-ICUBUNIT-canprotocol-sensorboards.docx and b/emBODY/eBdocs/arch-arm/TSD-ICUBUNIT-canprotocol-sensorboards.docx differ