OpenShoemodules
John-OlofNilssonJanuary30,2015
Thisdocument
ThisdocumentsdescribeshowtocontrolandcommunicatewiththeOpenShoemodules.Thegeneralstructureofthepackagesandthelogicsofthecommunicationaredescribedanddetailsofthedifferentcommands,states,andprocessingfunctionsandtheirfunctionalityaregiven.
Thedocumentdescribesthecommunicationprotocolasofrepos-itoryrevisions2f7ce4cande48a22efortheruntimeenvironmentandthealgorithmlibrary,respectively.Forearlierrevisionsthereisnodocumentationapartfromthecodeitself.However,thecommunica-tionlogicissimilarbutsomecommandsdiffer.
Ifsomethingdoesn’tworkasdescribed,mostlikelythedocumen-tationiswrongorthedocumentationdescribesthefunctionalityofanolderornewerversionofthecode.Havealookintheembeddedcodeandupdateyourmodule!Ifthingsstilldoesnotmakesense,ifrele-vantinformationismissing,orifyouhaveanysuggestionsconcerningthisdocument,sendane-mailtoopenshoe@ee.kth.se.
1
Contents
1Introduction
2Settingupthecommunication
3Commandandresponsestructures
3.1Commandstructure........................3.2Responsestructure........................3.3Checksum.............................4Receptionandtransmissionlogics
4.1Commandreception.......................4.2Responsetransmission......................5Commands
Auxiliarycommands
5.1Packageacknowledgement(0x01)................5.2Ping(0x03)............................5.3ModuleID(0x04).........................Debuggingcommands
5.4Setupdebugprocessingandoutput(0x10)...........5.5InputrawIMUdata(0x11)...................5.6Setstate(0x12-0x17).......................Request-datacommands
5.7Requestoutputofstate(0x20)..................5.8Requestoutputofmultiplestates(0x21)............5.9Turnoffalloutput(0x22)....................5.10Conditionaloutputsetup(0x23).................5.11OutputrawIMUdata(0x28)..................Request-processingcommands
5.12Runprocessingfunction(0x30).................5.13Runmultipleprocessingfunctions(0x31)............5.14Stopallprocessing(0x32)....................5.15ResetZUPT-aidedINS(0x33)..................5.16Step-wisedeadreckoning(0x34).................5.17Startinertialfrontend(0x35)..................5.18Restoreprocesssequencesetup(0x36)..
...........2
5556677788
91011121314161820212224252627283031
5.19Storeandemptyprocesssequence(0x37)........5.20Restoreprocesssequence(0x38).............Combinedcommands
5.21UseasnormalIMU(0x40)................5.22UseasnormalIMUwithonlinebiasestimation(0x41)....32....33....34....35
6States
Systemstates
6.1IMUtimestamp(0x01).....................6.2Interruptcounter(0x02).....................6.3Mainlooptimedifferential(0x03)................6.4ModuleID(0x04).........................6.5GeneralpurposeID(0x05)....................Inertialfrontendstates
6.6Combinedinertialreadings–preproc(0x10)..........6.7Combinedinertialreadings–statdet(0x11)..........6.8Timestampof0x11(0x12)....................6.9Combinedinertialreadings–floats(0x13)...........6.10Timedifferential(0x14).....................6.11Gaussianerrormodelteststatistics(0x15)...........6.12Gaussianandbiaserrormodelteststatistics(0x16)......6.13Stationaritydetectionbasedon0x15(0x17)..........6.14Stationaritydetectionbasedon0x16(0x18)..........Filteringstates
6.15Position(0x20)..........................6.16Velocity(0x21)..........................6.17Orientation(0x22)........................6.18Filtererrorcovariance(0x23)..................6.19Initialization-doneflag(0x24)..................Deadreckningstates
6.20Step(0x30)............................6.21Steperrorcovariance(0x31)...................6.22Stepcounter(0x32)........................6.23Filter-resetflag(0x33).................
.
.
.
.
.
RawIMUdatastates
6.24RawinertialreadingsfromIMUs(0x40-0x5F).........6.25RawtemperaturereadingsfromIMUs(0x60-0x7F)
......3
36
3636363737373738383838393939393940404040404141
4141
7Processingfunctions42
Auxiliaryprocessfunctions
7.1Storeandemptyprocesssequence(0x01)............7.2Restoreprocesssequence(0x02).................7.3Emptyprocesssequence(0x03).................7.4Conditionalrestoreofprocesssequence(0x04).........7.5Conditionalstateoutput(0x05).................7.6Singleconditionalstateoutput(0x06)..............7.7Conditionalstateoutput–counter(0x07)...........Frontendprocessfunctions
7.8Frontendpreprocessing(0x10)..................7.9Frontendstatisticscalculation(0x11)..............7.10Frontendpostprocessing(0x12).................Aidedinertialnavigationfunctions
7.11Mechanization(0x20).......................7.12Timeupdate(0x21)........................7.13Zero-velocityupdate(0x22)...................7.14Initialalignment(0x23).....................Deadreckoningfunctions
7.15Stepwisesystemreset(0x24)..........
.........4
42424242424243
43434343434344
44
1Introduction
TheOpenShoemodules(MIMU22BT,MIMU3333,MIMU4444)arecon-trolledbyasetofcommands.Thecommandscanbesentovereithercom-municationinterfaces,USBorBluetooth(UART).Thecommandssupportedbytheembeddedcodeandtheirfunctionalityisdescribedinthisdocument.Acommandcanessentiallydothreedifferentthings:itcanrequestastateasingletimeoratsomerate,itcanchangeastateofthemodule,anditcanmanipulatetheprocessfunctionsequencetomodifytheprocessingranateachtimeinstant.Thebehaviourofacommandiscontrolledbyaconnectedresponsefunction,whichisruninresponsetothecommandbeingreceivedbythemodule,andthecommandargumentswhicharepassedtotheresponsefunction.
Itispossiblefortheprocessfunctionsintheprocesssequencetomodifytheprocesssequenceitselfandtorequesttheoutputofsomestate.Forexam-ple,aprocessfunctionmayrequestastatetobeoutputwhensomeconditiondetectedbythefunctionappears.Therearenumerousutilityfunctionsandcommandsimplementedwhichwillperformsuchactions.
2Settingupthecommunication
TheUSBisconfiguredtoappearasavirtualcom-portonconnection(USBCDC).ForWindowsaninf-fileisrequiredforsetup(foundhere)thefirsttimethedeviceisconnected.ForLinuxandAndroidthesetupshouldbeautomatic.
ForcommunicationoverBluetooththedeviceneedtobepairedwiththeapplicationplatform.Thedeviceappearsasavirtualcom-port(SPPprofile).Possiblyitwillalsoappearasaheadset.Thisinterfacecanbeignored.Nospecialsetupshouldberequired.ForAndroid,themodulewillbeenumeratedwithalongid-number.ThisnamecanbefoundonWindowsifyoulookatpropertiesforyourpairedmodule.
3Commandandresponsestructures
Allthecommandsandresponsesstartwithasinglebyteheaderandendwithatwobytechecksum.Theheaderisdifferentfordifferentcommandsanddifferentresponses.Alldataisinbig-endianformat.Thestructureof
5
commandsandresponsesandhowtocalculatethechecksumisdescribedinthefollowingsubsections.Thecommandsaredescribedinhexadecimal.
3.1Commandstructure
Thecommandsstartwithaonebytecommandspecificheaderfollowedbyavaryingsizepayloadandatwobytechecksum.Thepayloadcanbedifferentfordifferentcommandsbutisfixedforaspecificcommand.Thecommandstructureisillustratedbelow.Eachblockcorrespondtoonebyteintheserialcommand.
CH............CKCK
CHcorrespondtothesinglebytecommandheaderandthetwoCKtothetwochecksumbytes.Thepayloadinbetweencontainstheargumentsofthecommandandisindicatedby...bytes.Asingleargumentmayspanmultiplebytes.
3.2Responsestructure
Theresponsetoacommandisoftwotypes.Acommandacknowledgement(ACK)oradatapackage.Theacknowledgementtoacommandisa(data)packagewithasinglebyteheader0xA0,asinglebytepayloadcorrespondingtothereceivedcommandheaderandatwobytechecksum.Theacknowl-edgementtothecommand03(ping)willlooklike
A00300A3
Thedatapackagesaretransmittedatsomeevenrate,asaresultofpro-cessingsetupbysomecommandortriggeredbysomeconfiguredcondition.Alldatapackageshaveacommonstructure.Theresponsesstartwithaonebyteheader0xAAfollowedbyatwobytepackagenumber(N1andN2),asinglebytepayloadsizeinbytes(SZ),thepayload,andatwobytechecksum.Theresponsestructureisillustratedbelow.
AAN1N2SZ.........CKCK
6
Notethatthepayloadsizeisonlyasinglebyteandforlargepackagesthisbytemayoverflow.However,currentlytheonlysensiblecaseduringwhichanoverflowmayoccuriswhenrawIMUdatafromindividualIMUsarerequested.Sincesuchdatawillonlyberequestedfortesting,suchanoverflowisacceptableandwehavechosentostaywithasinglebytepayloadsize.
3.3Checksum
Thechecksumisasimple16-bitmodularadditionofallproceedingbytesinthecommand/responsestoredinbig-endianformat.
4Receptionandtransmissionlogics
Thecommunicationlogicsimplementedbythemoduleisdescribedinthefollowingsubsections.
4.1Commandreception
Whenreceivingabyte,themodulewillcheckifthisbytecorrespondstoaheader.Ifnotitwillsimplydiscardthebyteandreturntolistenforaheader.Ifitisaheaderitwillcheckhowmanybytestoexpectandtrytoreadthisnumberofbytes.Ifitcannotreadthenumberofbyteswithinatimeoutlimit,itwilldiscardthebytesithasreadandreturntolistenforanewheader.Ifthenumberofexpectedbytesarereceived,itwillcheckthechecksum.Ifthechecksumisvalid,itwilltrytoparsethepayload.Ifthepayloadisvaliditwillpassittoaresponsefunctionconnectedtoeachcommand.Ifthechecksumisvalid,theparsingsucceeds,andthecommandisnotapackageacknowledgement,andACKwillbepreparedtobesentbacktotheapplicationplatform.Followingthisthemodulewillreturntolistenforanewheader.
CommandscanbesimultaneouslyreceivedoverboththeUSBandBlue-toothinterfaces.Theresponsestothecommandswillbesentbackoverthesameinterfaceastheywerereceived.Theexceptionisthedebugfunctionswheretheresponseinterfacemaybechosen.
7
4.2Responsetransmission
AteachtimeinstantthemodulewillcheckifthereisanyACKandstatesitshouldoutputtotheuser.Ifthelatteristhecase,itwillassemblethedatapackageinabuffer.AnACKispreparedbythecommandreceptionlogicsandresideinaseparatebuffer.Inthedatapackage,thestateswillbeaddedintheorderoftheirIDs.Onceapackageisassembledthepackagenumberandthesizeisaddedandthepackagenumbercounterincremented.ThebufferwillthenbepushedtheACK-bufferandthedatapackagebuffertoanoutputbuffer.Iftheoutputbufferisfull,thepackageswillbedropped.Forthewirelessinterfacethereistwomodesofcommunication,alossymodeandalosslessmode.Inthelossymode,whichisidenticaltotheUSB,thedatawillsimplybetransmittedandforgottenabout.Inthelosslessmode,thepackagewillbeaddedtoapackageque.Themodulewillthenkeeptransmittingtheoldestpackageuntilitgetacorrespondingpackageacknowledgementbackafterwhichitwillberemovedfromthequeandthesecondoldestpackagetransmitted.Ifthemodulesreceivesnopackageac-knowledgements,thepackagequewillfillupandpackageswilleventuallybedropped.
5Commands
Onthefollowingpages,thedifferentcommandsaredetailed.Thecommandscomesin4groups:auxiliary,debugging,requestdata,requestprocessingandcombinedcommands.Theauxiliarycommandsarecomposedofall”small“commands.Thedebuggingcommandsareusedtotestanddebugprocessingfunctionsonthemodule.Therequestdatacommandswillgivesometypeofoutputofthestateofthemodule.Theprocessingrequestcommandswillmakesomeprocessingbeingrunonthemodule.Thismayalsotriggersomeoutput.Thecombinedcommandswillrequestsomeoutputandsomeprocessing.Typically,thecombinedcommandsareshortcutstoachievebehaviourwhichcouldhavebeenattainedbyacombinationofothercommands.
Thecommandheaderisgiveninthesectiontitle.Thecommandargu-mentandthecommanditselfisdescribed.Finally,examplesofthecommanditselfandresponsesaregivenforeachcommand.
8
5.1
5.1.1
Packageacknowledgement(0x01)
Arguments
1.Packagenumber(2bytes)5.1.2
Description
Acknowledgementofreceptionofpackagesentbacktomoduletosignalthatuserhasreceivedthepackageindicatedintheargument.Thisisnecessarywhenusingthelosslesscommunicationmodeforthewirelessinterface,orthemodulewilljustkeepsendingtheoldestpackage.
Beinganacknowledgementinitself,thiscommandwillnotbefollowedbyanacknowledgementfromthemodule.Theactionfromthemoduleisthatthepackage(ifthepackageindicatedbytheargumentisthefirstpackageintheque)isremovedfromthepackageque.5.1.3
Examplecommand
0100010002
Theabovecommandwillsignalthetheuserhasreceivedpackagewithnum-ber0x0001.5.1.4
Exampleresponse
Noresponseisgivenbythemodule.
9
5.2
5.2.1None.5.2.2
Ping(0x03)
Arguments
Description
PingsthemodulewhichwillrespondwithanACK.Themodulewilltakenofurtheraction.5.2.3
Examplecommand
030003
Thisistheonlyformofthecommand.5.2.4
Exampleresponse
a00300a3
Nofurtherresponsetothecommandwillbegiven.
10
5.3
5.3.1None.5.3.2
ModuleID(0x04)
Arguments
Description
Themodulewilloutputaunique15byteserialnumberofthemicrocontroller.Thismaybeusedtoidentifythemodule.Theresponseissubjecttonormalheader,packagenumbersandchecksums.5.3.3
Examplecommand
040004
Thisistheonlyformofthecommand.5.3.4
Exampleresponse
Acknowledgementa00400a4followedbyaa000120
ff
0fd1f5
6f
00514b32344e2020
110c05bb
11
5.4
5.4.1
Setupdebugprocessingandoutput(0x10)
Arguments
1.8singlebyteprocessfunctionIDs(zeromeansnofunction)2.8stateIDswhichwillbeoutput(zerogivesnooutput)
3.1bytedeterminingtheinterfacethedebugoutputshouldbetransmit-tedover5.4.2
Description
Configuresandstoresaprocesssequenceofupto8processfunctionswhichwillberuneverytimerawIMUdataispushedtothemicrocontroller(com-mand0x11).Theprocessfunctionsarerunintheordertheyaregiven.ConfiguresasetofstateswhichwillbeoutputeverytimerawIMUdataispushedtothemicrocontroller.Ifthefirstbitofthelastargumentisset,theoutputwillbegivenovertheUSB.Ifthesecondbitofthelastargumentisset,theoutputwillbegivenoverBluetooth.Bothbitsmaybeset.5.4.3
Examplecommand
Thefollowingcommandwillsetupprocessfunctions0x10,0x11,and0x12(theinertialfrontend)toberunandtheresultingcombinedinertialmea-surementwithstateid0x13tobeoutputeverytimerawIMUdataispushedtothemicrocontroller.
101011120000000000130000000000000000ea5.4.4
Exampleresponse
Acknowledgementa01000b0
Thedataoutputisdescribedinthecommand0x20.
12
5.5
5.5.1
InputrawIMUdata(0x11)
Arguments
1.4bytetimestamp
2.6bytedataforeachIMU(theamountofdatawillvarydependingonwhichboardthecodeiscompiledfor)5.5.2
Description
InputrawIMUdatawhichisusedtooverwritethedatavaluesreadfromtheIMUs.Followingthistheresponsefunctionwillrestoretheprocesssequencesetupbythesetup-debug-processingcommand.Thiscommandisintendedfortesting(debugging)processingfunctionsontheboard.
Ifthesetupcommandhasnotbeenrunbeforethiscommandorifthestore_and_empty_process_sequence()-functionisruninbetween,there-storewillnotworkandthecommandmaygiveunintendedbehaviour.5.5.3
Examplecommand
Examplecommandfor4IMUs
1127484d940062008d0757ff
d8
ff
6c
ff
92
f7
ff
e6ff7e
ff
f8
5300190011fff1
e8
ff
b6
ff
fd00ff
85
5e0083079e0001f75.5.4
9c
ff
ff
ff
ef
ff
1e60
Exampleresponse
Acknowledgementa01000b0
followedbyoutputwhichisdependentonthesetupcommand.
13
5.6
5.6.1
Setstate(0x12-0x17)
Arguments
1.1bytestateID
2.1-2byteswithstatevalues5.6.2
Description
Thesecommandswilloverwritethevalueofthestate,indicatedbythefirstargument,withthefirstbytesofthesecondargument,correspondingtothesizeofthestate.Ifthesecondargumentisnotsufficientlylarge,nooverwritewilltakeplace.
Thedifferentcommandstakeadifferentsizeofthesecondargument:1.0x12–1byte2.0x13–4bytes3.0x14–12bytes4.0x15–24bytes5.0x16–48bytes6.0x17–2bytes5.6.3
Examplecommand
Settingthevalueofasinglebytestate(filterresetflag)1233010046
Settingthevalueofa4-bytestate(zero-velocityteststatistics)131502010101002d
Settingthevalueofa12-bytestate(position)
14200101010101010101010101010040
14
5.6.4Exampleresponse
Acknowledgementforsetstateofasinglebytestatea00c00ac
Nofurtherresponsetothecommandwillbegiven.
15
5.7
5.7.1
Requestoutputofstate(0x20)
Arguments
•1bytestateID•1byteoutputmode5.7.2
Description
RequeststhestateconnectedtothestateIDtobeoutput.Theoutputmodebytecontrolshowthestateisoutput.ThestatesareorderedaccordingtotheirIDsintheresponse.
Ifthe6thbitoftheoutputmodebyteisset,thestatewillbeoutput(pulled)asingletime.
Ifthe6thbitofthemodeselectorisnotset,thestatewillbeoutputatarateequaltothefullrate(nominally1000Hz)dividedby2x−1,wherexistheratedividerwhichisthe4leastsignificantbitsofthemodeselector.Ifthestateisalreadysettobeoutputatsomerate,thecommandwillchangethisrate.Aratedividerof0willturnofftheoutput.
Iftherequestissentoverthewirelessinterface,the5thbitoftheoutputmodebytewillcontroliflossy(notset)orlossless(set)transmissionisused.Thiswillapplytoallsubsequentoutput.
Theoutputisautomaticallysynchronizedsuchthatalloutputoflowerratecoincidewiththoseofhigherrates.Whenastateispulled,thestatewillbeoutputimmediately.5.7.3
Examplecommand
2001200041
TheIMUtimestamp(ID0x01)ispolledasingletime.5.7.4
Exampleresponse
Acknowledgementa02000c0followedby
16
aa0676041c
fb65d9037f
17
5.8
5.8.1
Requestoutputofmultiplestates(0x21)
Arguments
•8bytestateIDs•1byteoutputmode5.8.2
Description
RequeststhestatesconnectedtothestateIDstobeoutput.Theoutputmodebytecontrolshowthestateisoutput.ThestatesareorderedaccordingtotheirIDsintheresponse.Thiscommandisthesameas0x20justwithmoreIDs.IfzerosaregiveninsteadoftheIDs,nooutputwillbegiven.Consequently,thecommandmaybeusedtorequestoutputofupto8states.Formoreinformationaboutthemodebyte,seethe0x20command.5.8.3
Examplecommand
211011151600000000040071
Thecombinedinertialreadings0x10and0x11andteststatistics0x15and0x16isrequestedwitharateof125Hz(ratedivider0x04)(lossyifsentoverBluetooth).5.8.4
Exampleresponse
Acknowledgementa02100c1followedbyforexampleaa0500
ff
affe
380018c400000d30008800
ff
fcfc
b800
ff
fc
2f
88
fd9400001aff
ff
8000
ff
6800000b8000fab800
ff
2e3800
fd4000000266a400000173
18
1784
19
5.9
5.9.1None.5.9.2
Turnoffalloutput(0x22)
Arguments
Description
Turnsoffallevenrateoutputandoutputsetuptobetriggeredbyaflag.ThecommandalsoemptiesthepackagequeoftheBluetoothinterface.Outputwhichistriggeredbyprocessingfunctionsmaystillappear.5.9.3
Examplecommand
2200225.9.4
Exampleresponse
a02200c2
Nofurtherresponsetothecommandwillbegiven.
20
5.10
5.10.1
Conditionaloutputsetup(0x23)
Arguments
1.1bytestateIDoftriggeringstate2.1byteoutputmode
3.8bytesstateIDstobeoutput5.10.2
Description
Configuresthestateindicatedbyargument1(thefirstbyteofthestatebeingnon-zero)totriggertheconditionaloutpututilityprocessfunction(0x03)tosetanoutputwithamodegivenbyargument2andstatesgivenbyargument3.5.10.3
Examplecommand
Setupthezero-velocityflagtotriggeranoutputofitself.231720170000000000000000715.10.4
Exampleresponse
a02300c3
Nofurtherresponsetothecommandwillbegiven.Thiscommandsonlysetuptheconditionsunderwhichtheoutputistriggeredandwhattheoutputis.Togettheactualoutput,theprocessfunction0x03mustberunandtheconditionmostbetrue.
21
5.11
5.11.1
OutputrawIMUdata(0x28)
Arguments
1.4bytebit-fieldindicatingfromwhichoutof32IMUsthedatashouldbeoutput2.1byteoutputmode5.11.2
Description
TellsthemoduletooutputtherawdatareadfromtheIMUstogetherwithacommontimestamp(4bytes).The32bitsinthebit-fieldcorrespondtotheuptopotentially32IMUsofthemodules.AsetbitwillgivethedatafromthecorrespondingIMU.Ifabitissetwhichcorrespondstoanon-existingIMU,zeroswillbeoutput.(Thestatesarestilltherebutnodataiseverwrittentothestates.)Thetimestamp(state0x01)isa32-bitclockregisterreadwhenthedataisreceivedfromtheIMUs.WithaMHzclockfrequency,thistimestampwillwrapevery67s.
Theoutputmodebyteworksthesamewayasforthegeneralstateoutputcommandbutinadditiontotheratedivider(1st-4thbit),thelossy/losslessbit(5thbit)andthesingle/evenratetransmissionbit(6thbit),the7thand8thbitindicateiftherawinertialand/orthetemperaturereadingsoftheIMUsshouldbeoutput.Theinertialdatais2×6bytesperIMUandthetemperatureis2bytesperIMU.5.11.3
Examplecommand
0f
410078
28000000
Requestoftherawinertialdata(nottemperature)fromthefirst4IMUs(theIMUsoftheMIMU22BTmodules)atarateof125Hz.IfsentoverBluetooth,lossytransmissionmodewillbeused.5.11.4
Exampleresponse
Acknowledgementa02800c8
22
Followedbyforexample
aa19d6343d78025e0001ff95
fffbff
ea00090009fff8
ef
ff
ff
7f
0002
ff
f7f5
a600ff
f0
07ac
77079d00110001002600ff
f5
ff
f2
00191b82
f7d2
23
5.12
5.12.1
Runprocessingfunction(0x30)
Arguments
1.1bytefunctionID2.1bytearraylocation5.12.2
Description
Addstheprocessfunctionindicatedbythefirstargumenttothearrayloca-tionindicatedbythesecondargument.Thearraylocationmustbebetween0and10.Alargervaluewillgivenoeffect.Anon-existing(orzero)functionIDwillremoveanyprocessingfunctioninthearraylocation.5.12.3
Examplecommand
30100036
Turnsontheinertialfrontendpreprocessingpart(locatetheprocessingfunc-tioninthefirstprocessingsequenceslot)whichwillcombinetherawinertialmeasurementsfromdifferentIMUstoasinglecombinedreading.5.12.4
Exampleresponse
Acknowledgementa03000d0
Nofurtherresponsewillbegiven.However,someprocessfunctionwilltriggerfurtheroutput.
24
5.13
5.13.1
Runmultipleprocessingfunctions(0x31)
Arguments
1.8bytefunctionIDs5.13.2
Description
Addsthe8processfunctionsindicatedbythefirstargumenttothearraylocation0-7.Anon-existing(orzero)functionIDwillremoveanyprocessingfunctioninthearraylocation.5.13.3
Examplecommand
31101112000000000000Turnsontheinertialfrontend.5.13.4
Exampleresponse
Acknowledgementa0
1f
00bf
Nofurtherresponsewillbegiven.
25
5.14
5.14.1None5.14.2
Stopallprocessing(0x32)
Arguments
Description
Thecommandemptiestheprocessingsequencestoppingallfurtherprocess-ing.5.14.3
Examplecommand
320032
Thisistheonlyformofthecommand.5.14.4
Exampleresponse
Acknowledgementa03200d2
Nofurtherresponsewillbegiven.
26
5.15
5.15.1None5.15.2
ResetZUPT-aidedINS(0x33)
Arguments
Description
Starts(resets)theindefiniteZUPT-aidedINS.Ashortinitialalignment(256sufficientlystationarysamples)isperformedafterwhichtheprocessingstart.Thiscanbecombinedwitharequestforthepositionstatewithsomeratetoletthemoduletracktheuserwithoutanyprocessingonhisside.However,ingeneral,thestep-wiseversionispreferableandthiscommandisprimarilykeptforcompleteness.5.15.3
Examplecommand
330033
Thisistheonlyformofthecommand.5.15.4
Exampleresponse
Acknowledgementa03300d3
Nofurtherresponsewillbegiven.
27
5.16
5.16.1None5.16.2
Step-wisedeadreckoning(0x34)
Arguments
Description
Starts(resets)step-wisedeadreckoningwhichisjusttheZUPT-aidedINSbutwithresetsandtriggeredoutputateverystep.Firstashortinitialalignment(256sufficientlystationarysamples)isperformedafterwhichtheprocessingstart.Theinitialalignmentperiodwillalsoletallbuffersfillupandaninitialbiasestimatetobecomputed.
Oncetheprocessingisupandrunning,adisplacementandheadingchangeestimatetogetherwithcorrespondingerrorcovarianceestimateispushedtotheusereverytimethemodulebecomessufficientlystationaryandatanevenrateifstationaryforanextendedperiod.These”steps“canbeusedtoperformdeadreckoningonanapplicationplatform.Forfurtherdetails,seethearticledescribingtheOpenShoemodules.5.16.3
Examplecommand
340034
Thisistheonlyformofthecommand.5.16.4
Exampleresponse
Acknowledgementa03400d4
Followedbystepoutputlikeaa002a3a3cae7dbe9659a73796e732
fef0
a73e7ecbbebd498124e3f0
af
e031de311b
f0da5537194932da48e2b1
28
19bc2737000b1ec1
efb11bada1524a3483b8df
29
5.17
5.17.1None5.17.2
Startinertialfrontend(0x35)
Arguments
Description
Placestheinertialfrontendprocessingfunctioninlocation0-2intheprocess-ingsequence.ThesefunctionswillcombinetherawinertialreadingsfromthedifferentIMUsandcompensateforthecalibration(ifavailable),calcu-latethetestzero-velocitystatisticsandperformon-linebiascalibrationandconversiontofloatingpointvalueswithSIunits.Thiscommandwillenabletheusertorequestcombinedinertialreadings.5.17.3
Examplecommand
350035
Thisistheonlyformofthecommand.5.17.4
Exampleresponse
Acknowledgementa03500d5
Nofurtherresponsewillbegiven.
30
5.18
5.18.1
Restoreprocesssequencesetup(0x36)
Arguments
1.1bytestateIDoftriggeringstate5.18.2
Description
Configuresthestateindicatedbyargument1(thefirstbyteofthestatebeingnon-zero)totriggertheresettoastoredprocesssequencebytheutilityprocessfunction0x04.Settingupandstoringaprocesssequencecanbedonebyaddingtheprocessfunctionstotheprocesssequenceandcallingthestoreandemptyprocesssequencecommand(0x37).5.18.3
Examplecommand
Setupthezero-velocityflagtotriggertherestorationoftheprocesssequence.3617004d5.18.4
Exampleresponse
a03600d6
Nofurtherresponsetothecommandwillbegiven.
31
5.19
5.19.1None.5.19.2
Storeandemptyprocesssequence(0x37)
Arguments
Description
Savestheprocesssequenceandemptiestheprocesssequence.5.19.3
Examplecommand
370037
Thisistheonlyformofthecommand.5.19.4
Exampleresponse
a03700d7
Nofurtherresponsetothecommandwillbegiven.
32
5.20
5.20.1None.5.20.2
Restoreprocesssequence(0x38)
Arguments
Description
Restoresavedprocesssequence.5.20.3
Examplecommand
380038
Thisistheonlyformofthecommand.5.20.4
Exampleresponse
a03800d8
Nofurtherresponsetothecommandwillbegiven.
33
5.21
5.21.1
UseasnormalIMU(0x40)
Arguments
1.1byteoutputmode5.21.2
Description
ThiscommandwillconfigurethemoduletoworkasanormalIMU.ThereadingsfromthedifferentIMUswillbecombinedandfloatingpointreadings(0x13)willbeoutputaccordingtotheoutputmodebyte.
Formoreinformationabouttheoutputmodebyte,seecommand0x20.5.21.3
Examplecommand
40030043
Requestofcombinedinertialreadingatfullrate.5.21.4
Exampleresponse
Acknowledgementa04000e0followedbyoutputlike
aa00011c17dd3a5d3f7bc115
02a24b3c
cf
3c
8fd2bb87218cbc166345bbae
5cd60d7d
34
5.22
5.22.1
UseasnormalIMUwithonlinebiasestimation(0x41)
Arguments
1.1byteoutputmode5.22.2
Description
ThiscommandwillconfigurethemoduletoworkasanormalIMUwithonlinegyrobiasestimationandcompensation.Gyrobiasesareestimatedbasedontimeinstantswheretheinertialreadingsinatimewindowshowsanoisefloorcharacteristics.Thebiasestimateswillbesubtractedfromthecombinedinertialreadings.Sincethebiasesareestimatedonlinethereisa127sampledelay(approximately0.127s)inthereceivedreadings.
Formoreinformationabouttheoutputmodebyte,seecommand0x20.5.22.3
Examplecommand
41030044
Requestofcombinedinertialreadingatfullrate.5.22.4
Exampleresponse
Acknowledgementa04100e1
Theoutputwillbethesameasfor0x40butthegyrobiasshouldreturntozeroswiftlyastheIMUbecomesstationary.
35
6States
Thestatesarevariableswhichcanbeaccessedbytheuser.Thestatesarealsousedbytheprocessingfunctionstocommunicatesotheyrepresent,toalargeextent,theinformationwhichispropagatedthroughtheprocessing.Ofcourse,thecompletestateofthemoduleismuchlargerbutwillbeinacces-siblewithoutadebugger.EachstateisidentifiedbyasinglebytestateID.Thestatesaredeclaredexternintheheaderfilebelongingtotheplacewheretheyareused.Theseheaderfilesareincludedinthefilesystem_state.cwhichkeepsatableofallstateswhichisusedbythesystem.
6.1IMUtimestamp(0x01)
StateID:0x01Type:uint32_tSize:4bytes
TimestamptakenbeforeeachreadoperationtotheIMU.
6.2Interruptcounter(0x02)
0x02
uint32_t4bytes
StateID:Type:Size:
Acounterwhichisincrementedbyoneeachtimethedrivinginterruptissetoff.
6.3Mainlooptimedifferential(0x03)
0x03
uint32_t4bytes
StateID:Type:Size:
Timedifferentialindicatingtheexecutiontimeofthelastmain-loop.
36
6.4ModuleID(0x04)
0x04
char[15](seedescriptionbelow)15bytes
StateID:Type:Size:
15byteserialnumberofthemodule(ofthemicrocontroller).
Thisstateisnotmanifestedbyapropervariablebutratherapointertoanaddressinawriteprotectedmemory.Thereforethestatecannotbeset.
6.5GeneralpurposeID(0x05)
0x05uint8_t1bytes
StateID:Type:Size:
GeneralpurposeIDstatewhichcanbeset.
6.6Combinedinertialreadings–preproc(0x10)
0x10
inert_int32(struct)24bytes
StateID:Type:Size:
Combinedinertialreadingsprovideddirectlybytheinertialfrontendprepro-cessing.Thestatecontains6chunksof4bytes(int32_t).First3chunksof4bytesgivingthecombinedspecificforcereadingandthen3chunksof4bytesgivingthecombinedangularratereadings.
6.7Combinedinertialreadings–statdet(0x11)
StateID:0x11Type:inert_int32(struct)Size:24bytes
Combinedinertialreadingsprovidedbytheinertialfrontendstatisticscal-culations.Thesereadingsaresynchronizedtheprovidedteststatisticsandshouldbeusedincombinationwiththese.Thereadingsareusedbytheinertialfrontendpostporcessingfunction.Thereadingsexhibitsadelayofhalfthenumberofsamplesofthelongestteststatisticswindow.Thestatecontains6chunksof4bytes(int32_t).First3chunksof4bytesgiving
37
thecombinedspecificforcereadingandthen3chunksof4bytesgivingthecombinedangularratereadings.
6.8Timestampof0x11(0x12)
0x12
uint32_t4bytes
StateID:Type:Size:
Timestampofthecombinedinertialreadings0x11.Thesourceofthesetimestampsare0x01.Comparedwith0x01thiswillshowthedelayof0x12.
6.9Combinedinertialreadings–floats(0x13)
0x13
inert_float(struct)24bytes
StateID:Type:Size:
Combinedinertialreadingsconvertedtofloatingpoint.Ifonlythefrontendpreprocessingisrun,thesereadingswilloriginatefrom0x10.Ifthecompletefrontendisrun,thesereadingswilloriginatefrom0x11andexhibitthesamedelay.Thestatecontains6chunksof4bytes(singleprecisionfloats).Firstfloatsgivingthecombinedspecificforcereadingandthen3floatsgivingthecombinedangularratereadings.
6.10Timedifferential(0x14)
StateID:0x14Type:floatSize:4bytes
Timedifferentialofthecombinedinertialreadings0x11.Thesetimediffer-entialsareusedinthemechanization.
6.11Gaussianerrormodelteststatistics(0x15)
StateID:0x15Type:uint32_tSize:4bytes
StationarityteststatisticscalculatedbasedonanGaussianerrormodel.
38
6.12Gaussianandbiaserrormodelteststatistics(0x16)
0x16
uint32_t4bytes
StateID:Type:Size:
StationarityteststatisticscalculatedbasedonGaussianplusbiaserrormodel.
6.13Stationaritydetectionbasedon0x15(0x17)
0x17bool1bytes
StateID:Type:Size:
Thresholdedteststatistics0x15.
6.14Stationaritydetectionbasedon0x16(0x18)
0x18bool1bytes
StateID:Type:Size:
Thresholdedteststatistics0x16.
6.15Position(0x20)
0x20
float[3]12bytes
StateID:Type:Size:
Positionstate.
6.16Velocity(0x21)
0x21
float[3]12bytes
StateID:Type:Size:
Velocitystate.
39
6.17Orientation(0x22)
0x22
float[4]16bytes
StateID:Type:Size:
Platformorientationinquaternionrepresentation.
6.18Filtererrorcovariance(0x23)
0x23
float[45]180bytes
StateID:Type:Size:
Symmetricerrorcovarianceofthe9statesposition,velocity,andorientation(euleranglerepresentation).Sincethematrixissymmetriconly45valuesarestored.
6.19Initialization-doneflag(0x24)
0x24bool1bytes
StateID:Type:Size:
Flagsignalingwheninitialization(coarseinitialalignment)isdone.
6.20Step(0x30)
0x30
float[4]16bytes
StateID:Type:Size:
Displacementandheadingchangeextractedatfilterreset.
6.21Steperrorcovariance(0x31)
0x31
float[10]40bytes
StateID:Type:Size:
Errorcovarianceof0x30.
40
6.22Stepcounter(0x32)
0x32
uint16_t2bytes
StateID:Type:Size:
Countercountingtheresets(steps)sincepower-up.
6.23Filter-resetflag(0x33)
0x25bool1bytes
StateID:Type:Size:
Flagsignalingwhenthefilteringhasbeenreset(forstep-wisedeadreckon-ing).
6.24RawinertialreadingsfromIMUs(0x40-0x5F)
0x40-0x5Fint16_t[6]12bytes
StateID:Type:Size:
Inertialreadings,3specificforceand3angularratereadings,fromindividualIMUs.OnlythestatescorrespondingtoIMUsontherespectiveboardwillbefilledup.Theotherstateswillremainzero.StatesofunmountedIMUswillbefilledupwithones(duetopull-upresistors).
6.25RawtemperaturereadingsfromIMUs(0x60-0x7F)
0x60-0x7Fint16_t2bytes
StateID:Type:Size:
TemperaturereadingsfromindividualIMUs.
41
7Processingfunctions
Theprocessingfunctionsarefunctionswhichtheusercanaskthemoduleroruninaspecificorder.Thefunctionsmayemployotherfunctionstocompleteitstasksandcommandresponsefunctionsmayalsomakethemodulerunotherfunctions.However,theprocessfunctionsarethefunctionsvisibletotheuser.EachprocessfunctionisidentifiedbyasinglebyteID.Theprocessingfunctionsarebrieflydescribedbelow.
7.1Storeandemptyprocesssequence(0x01)
Temporarilystores(forlaterrestoration)andemptiestheprocesssequence.
7.2Restoreprocesssequence(0x02)
Restorestheprocesssequencestoredbythecommand0x01.Ifnoprocesssequencehasbeenstored,theprocesssequencewillbeoverwrittenbyanemptyprocesssequence.
7.3Emptyprocesssequence(0x03)
Emptiestheprocesssequence,consequentlyalsoremovingitself.
7.4Conditionalrestoreofprocesssequence(0x04)
Restorestheprocesssequencegiventhatarelatedstate(flag)istrue.Forfurtherinformationseecommand0x36.
7.5Conditionalstateoutput(0x05)
Flagsoneormorestatesforoutputgiventhatarelatedstate(flag)istrue.Forfurtherinformationseecommand0x23.
7.6Singleconditionalstateoutput(0x06)
Flagsoneormorestatesforoutputasingletimegiventhatarelatedstate(flag)istrue.Forfurtherinformationseecommand0x23.
42
7.7Conditionalstateoutput–counter(0x07)
Flagsoneormorestatesforoutputgiventhatarelatedcounterhassurpassedavalue.Forfurtherinformationseecommand0x23.
7.8Frontendpreprocessing(0x10)
Preprocessingparttotheinertialfrontend.Thisfunctioncombinestherawinertialreading(0x40-0x5F)andperformsthecalibrationcompensationwrit-ingtheresultsto0x10.Finally,thecombinedinertialreadingsareconvertedtofloatvaluesin0x13.
7.9Frontendstatisticscalculation(0x11)
Inertialdatabufferingandrecursivezero-velocitystatisticscalculation.Thefunctionshasinternalbuffersinwhichthecombinedinertialreading0x10arebufferedinordertocalculatethestatistics0x15and0x16andtherelatedstates0x17and0x18.Fromthebuffers,thefunctionprovides0x11and0x12.
7.10Frontendpostprocessing(0x12)
Onlinebiasestimationbasedonthecalculatedstatistic0x16,compensationappliedtothevalues0x11andconversionsoftheresultstofloats0x13.Thefunctionalsocalculatesthetimedifferential0x14.
7.11Mechanization(0x20)
Basicinertialmechanization.
7.12Timeupdate(0x21)
TimeupdateoftheKalmanfilter.
7.13Zero-velocityupdate(0x22)
Zero-velocityupdateoftheKalmanfilter.
43
7.14Initialalignment(0x23)
Initialalignmentfortheaidedinertialnavigation.Relyon0x17andprovidedinertialreadingin0x13toperformtheinitialalignment.Signalthatthealignmentisdonebysettingtheflag0x24.
7.15Stepwisesystemreset(0x24)
ChecksiftheZUPT-aidedINSshouldbereset,savesthestate,performstheresetandsignalthatanewstepisavailable.
44
因篇幅问题不能全部显示，请点此查看更多更全内容
Copyright © 2019- oldu.cn 版权所有 浙ICP备2024123271号-1
违法及侵权请联系：TEL:199 1889 7713 E-MAIL:2724546146@qq.com
本站由北京市万商天勤律师事务所王兴未律师提供法律服务