Skip to content

Commit

Permalink
Merge pull request #286 from icub-tech-iit/fix/strainEul
Browse files Browse the repository at this point in the history 
Fix euler angles representation for strain2
  • Loading branch information
@Nicogene
Nicogene authored Jul 28, 2022
2 parents 65f9ed0 + a9f530b commit 2aa2ecb
Show file tree
Hide file tree
Showing 5 changed files with 473 additions and 497 deletions.
134 changes: 67 additions & 67 deletions emBODY/eBcode/arch-arm/board/mtb4/application/src/main-appcan.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,15 +12,15 @@
// --------------------------------------------------------------------------------------------------------------------
// config start

constexpr embot::app::theCANboardInfo::applicationInfo applInfo
constexpr embot::app::theCANboardInfo::applicationInfo applInfo
{
#if defined(CUSTOMIZATION_MTB4_FOR_TLR)
embot::prot::can::versionOfAPPLICATION {20, 4, 5},
embot::prot::can::versionOfCANPROTOCOL {20, 0}
#else
embot::prot::can::versionOfAPPLICATION {1, 4, 6},
embot::prot::can::versionOfCANPROTOCOL {2, 0}
#endif
#if defined(CUSTOMIZATION_MTB4_FOR_TLR)
embot::prot::can::versionOfAPPLICATION {20, 4, 5},
embot::prot::can::versionOfCANPROTOCOL {20, 0}
#else
embot::prot::can::versionOfAPPLICATION {1, 4, 7},
embot::prot::can::versionOfCANPROTOCOL {2, 0}
#endif
};

constexpr std::uint16_t threadIDLEstacksize = 512;
Expand All @@ -37,8 +37,8 @@ static void *paramEVNT = nullptr;

constexpr embot::os::theTimerManager::Config tmcfg {};
constexpr embot::os::theCallbackManager::Config cmcfg {};


static const embot::app::skeleton::os::basic::sysConfig syscfg { threadINITstacksize, paramINIT, threadIDLEstacksize, paramIDLE, paramERR, tmcfg, cmcfg};

static const embot::app::skeleton::os::evthreadcan::evtConfig evtcfg { threadEVNTstacksize, paramEVNT, threadEVNTtimeout};
Expand All @@ -52,9 +52,9 @@ static const embot::app::skeleton::os::evthreadcan::canConfig cancfg { maxINPcan
class mySYS final : public embot::app::skeleton::os::evthreadcan::SYSTEMevtcan
{
public:
mySYS(const embot::app::skeleton::os::basic::sysConfig &cfg)
mySYS(const embot::app::skeleton::os::basic::sysConfig &cfg)
: SYSTEMevtcan(cfg) {}

void userdefOnIdle(embot::os::Thread *t, void* idleparam) const override;
void userdefonOSerror(void *errparam) const override;
void userdefInit_Extra(embot::os::EventThread* evtsk, void *initparam) const override;
Expand All @@ -64,13 +64,13 @@ class mySYS final : public embot::app::skeleton::os::evthreadcan::SYSTEMevtcan
class myEVT final : public embot::app::skeleton::os::evthreadcan::evThreadCAN
{
public:
myEVT(const embot::app::skeleton::os::evthreadcan::evtConfig& ecfg, const embot::app::skeleton::os::evthreadcan::canConfig& ccfg, const embot::app::theCANboardInfo::applicationInfo& a)
myEVT(const embot::app::skeleton::os::evthreadcan::evtConfig& ecfg, const embot::app::skeleton::os::evthreadcan::canConfig& ccfg, const embot::app::theCANboardInfo::applicationInfo& a)
: evThreadCAN(ecfg, ccfg, a) {}

void userdefStartup(embot::os::Thread *t, void *param) const override;
void userdefOnTimeout(embot::os::Thread *t, embot::os::EventMask eventmask, void *param) const override;
void userdefOnEventRXcanframe(embot::os::Thread *t, embot::os::EventMask eventmask, void *param, const embot::prot::can::Frame &frame, std::vector<embot::prot::can::Frame> &outframes) const override;
void userdefOnEventANYother(embot::os::Thread *t, embot::os::EventMask eventmask, void *param, std::vector<embot::prot::can::Frame> &outframes) const override;
void userdefOnEventANYother(embot::os::Thread *t, embot::os::EventMask eventmask, void *param, std::vector<embot::prot::can::Frame> &outframes) const override;
};


Expand All @@ -81,9 +81,9 @@ constexpr embot::app::skeleton::os::evthreadcan::CFG cfg{ &mysys, &myevt };
// --------------------------------------------------------------------------------------------------------------------

int main(void)
{
{
embot::app::skeleton::os::evthreadcan::run(cfg);
for(;;);
for(;;);
}


Expand All @@ -94,13 +94,13 @@ int main(void)
#include "embot_hw_bno055.h"

namespace embot { namespace hw { namespace bsp { namespace mtb4 {

constexpr embot::hw::SI7051 thermometer = embot::hw::SI7051::one;
constexpr embot::hw::si7051::Config thermometerconfig = {};

constexpr embot::hw::BNO055 imuBOSCH = embot::hw::BNO055::one;
constexpr embot::hw::bno055::Config imuBOSCHconfig = {};
constexpr embot::hw::bno055::Config imuBOSCHconfig = {};

}}}} // namespace embot { namespace hw { namespace bsp { namespace mtb4 {

#include "embot_os_theScheduler.h"
Expand All @@ -123,42 +123,42 @@ constexpr embot::os::Event evTHERMOdataready = 0x00000001 << 6;
void mySYS::userdefOnIdle(embot::os::Thread *t, void* idleparam) const
{
static int a = 0;
a++;
a++;
}

void mySYS::userdefonOSerror(void *errparam) const
{
static int code = 0;
embot::os::theScheduler::getInstance().getOSerror(code);
for(;;);
for(;;);
}


void mySYS::userdefInit_Extra(embot::os::EventThread* evtsk, void *initparam) const
{
// inside the init thread: put the init of many things ...
// inside the init thread: put the init of many things ...

// led manager
static const std::initializer_list<embot::hw::LED> allleds = {embot::hw::LED::one};
embot::app::theLEDmanager &theleds = embot::app::theLEDmanager::getInstance();
theleds.init(allleds);
theleds.get(embot::hw::LED::one).pulse(embot::core::time1second);
static const std::initializer_list<embot::hw::LED> allleds = {embot::hw::LED::one};
embot::app::theLEDmanager &theleds = embot::app::theLEDmanager::getInstance();
theleds.init(allleds);
theleds.get(embot::hw::LED::one).pulse(embot::core::time1second);

// init of can basic paser
embot::app::application::theCANparserBasic::getInstance().initialise({});
// init agent of skin

// init agent of skin
embot::app::application::theSkin &theskin = embot::app::application::theSkin::getInstance();
embot::app::application::theSkin::Config configskin;
configskin.tickevent = evSKINprocess;
configskin.totask = evtsk;
theskin.initialise(configskin);
theskin.initialise(configskin);

// init canparser skin and link it to its agent
embot::app::application::theCANparserSkin &canparserskin = embot::app::application::theCANparserSkin::getInstance();
embot::app::application::theCANparserSkin::Config configparserskin { &theskin };
canparserskin.initialise(configparserskin);
canparserskin.initialise(configparserskin);

// init agent of imu
embot::app::application::theIMU &theimu = embot::app::application::theIMU::getInstance();
embot::app::application::theIMU::Config configimu(embot::hw::bsp::mtb4::imuBOSCH, embot::hw::bsp::mtb4::imuBOSCHconfig, evIMUtick, evIMUdataready, evtsk);
Expand All @@ -167,48 +167,48 @@ void mySYS::userdefInit_Extra(embot::os::EventThread* evtsk, void *initparam) co
// init canparser imu and link it to its agent
embot::app::application::theCANparserIMU &canparserimu = embot::app::application::theCANparserIMU::getInstance();
embot::app::application::theCANparserIMU::Config configparserimu { &theimu };
canparserimu.initialise(configparserimu);
canparserimu.initialise(configparserimu);

// init agent of thermo
embot::app::application::theTHERMO &thethermo = embot::app::application::theTHERMO::getInstance();
embot::app::application::theTHERMO::Config configthermo(embot::hw::bsp::mtb4::thermometer, embot::hw::bsp::mtb4::thermometerconfig, evTHERMOtick, evTHERMOdataready, evtsk);
thethermo.initialise(configthermo);
thethermo.initialise(configthermo);

// init canparser thermo and link it to its agent
embot::app::application::theCANparserTHERMO &canparserthermo = embot::app::application::theCANparserTHERMO::getInstance();
embot::app::application::theCANparserTHERMO::Config configparserthermo { &thethermo };
canparserthermo.initialise(configparserthermo);
canparserthermo.initialise(configparserthermo);

}

void myEVT::userdefStartup(embot::os::Thread *t, void *param) const
{
// inside startup of evnt thread: put the init of many things ...
// inside startup of evnt thread: put the init of many things ...
}


void myEVT::userdefOnTimeout(embot::os::Thread *t, embot::os::EventMask eventmask, void *param) const
{
static uint32_t cnt = 0;
cnt++;
cnt++;
}


void myEVT::userdefOnEventRXcanframe(embot::os::Thread *t, embot::os::EventMask eventmask, void *param, const embot::prot::can::Frame &frame, std::vector<embot::prot::can::Frame> &outframes) const
{
{
// process w/ the basic parser. if not recognised call the parsers specific of the board
if(true == embot::app::application::theCANparserBasic::getInstance().process(frame, outframes))
{
{
}
if(true == embot::app::application::theCANparserSkin::getInstance().process(frame, outframes))
{
{
}
else if(true == embot::app::application::theCANparserIMU::getInstance().process(frame, outframes))
{
{
}
else if(true == embot::app::application::theCANparserTHERMO::getInstance().process(frame, outframes))
{
}
{
}
}

void myEVT::userdefOnEventANYother(embot::os::Thread *t, embot::os::EventMask eventmask, void *param, std::vector<embot::prot::can::Frame> &outframes) const
Expand All @@ -217,40 +217,40 @@ void myEVT::userdefOnEventANYother(embot::os::Thread *t, embot::os::EventMask ev
{
embot::app::application::theSkin &theskin = embot::app::application::theSkin::getInstance();
theskin.tick(outframes);

// we operate on the skin triangles by calling a skin.tick(outframes);
// the evSKINprocess is emitted by:
// 1. a periodic timer started at the reception of a specific message.


// the .tick(outframes) will do whatever it needs to do and it may emit some
// the .tick(outframes) will do whatever it needs to do and it may emit some
// can frames for transmission. the can frames can be up to 16x2 = 32.
// hence, how many packets? max of replies = 8 + max of broadcast = 32 --> 40.

}

if(true == embot::core::binary::mask::check(eventmask, evIMUtick))
{
{
embot::app::application::theIMU &theimu = embot::app::application::theIMU::getInstance();
theimu.tick(outframes);
}
theimu.tick(outframes);
}

if(true == embot::core::binary::mask::check(eventmask, evIMUdataready))
{
{
embot::app::application::theIMU &theimu = embot::app::application::theIMU::getInstance();
theimu.processdata(outframes);
}
theimu.processdata(outframes);
}

if(true == embot::core::binary::mask::check(eventmask, evTHERMOtick))
{
{
embot::app::application::theTHERMO &thethermo = embot::app::application::theTHERMO::getInstance();
thethermo.tick(outframes);
}
thethermo.tick(outframes);
}

if(true == embot::core::binary::mask::check(eventmask, evTHERMOdataready))
{
{
embot::app::application::theTHERMO &thethermo = embot::app::application::theTHERMO::getInstance();
thethermo.processdata(outframes);
thethermo.processdata(outframes);
}
}

Expand All @@ -260,10 +260,10 @@ void myEVT::userdefOnEventANYother(embot::os::Thread *t, embot::os::EventMask ev
#if defined(CUSTOMIZATION_MTB4_FOR_TLR)
#warning CUSTOMIZATION_MTB4_FOR_TLR is active: see what changes it takes
// the TLR customization changes the IDs of the CAN frames emitted periodically to broadcast the 12 values of the skin taxel so that:
// - the first frame has unchanged ID formed by the following three nibbles [cls | adr | trg]
// - the first frame has unchanged ID formed by the following three nibbles [cls | adr | trg]
// with cls = 4 = embot::protocol::can::Cls::periodicSkin, adr = address of transmitting mtb4 board, trg = number of the triangle.
// - the second frame has an ID different from standard protocol because the value of cls is now = 6 = embot::protocol::can::Cls::periodicForFutureUse
// it also has a embot::prot::can::versionOfCANPROTOCOL {20, 0}
// it also has a embot::prot::can::versionOfCANPROTOCOL {20, 0}
#endif


Expand Down
Loading

0 comments on commit 2aa2ecb

Please sign in to comment.