Browse Source

lots of work on MPU calibration

drive-to-target
Alex Mikhalev 6 years ago
parent
commit
3f724fce03
  1. 860
      calib/calib.nb
  2. 19
      calib/calib.py
  3. 12
      calib/plot.sh
  4. 47
      main/ugv_io_mpu.cc
  5. 2
      main/ugv_io_mpu.hh

860
calib/calib.nb

@ -0,0 +1,860 @@ @@ -0,0 +1,860 @@
(* Content-type: application/vnd.wolfram.mathematica *)
(*** Wolfram Notebook File ***)
(* http://www.wolfram.com/nb *)
(* CreatedBy='Mathematica 11.3' *)
(*CacheID: 234*)
(* Internal cache information:
NotebookFileLineBreakTest
NotebookFileLineBreakTest
NotebookDataPosition[ 158, 7]
NotebookDataLength[ 38929, 850]
NotebookOptionsPosition[ 37210, 812]
NotebookOutlinePosition[ 37546, 827]
CellTagsIndexPosition[ 37503, 824]
WindowFrame->Normal*)
(* Beginning of Notebook Content *)
Notebook[{
Cell[CellGroupData[{
Cell[BoxData[
RowBox[{"SetDirectory", "[", "\"\</home/alex/Programming/esp/uas-ugv\>\"",
"]"}]], "Input",
CellChangeTimes->{{3.764181122949832*^9, 3.764181135056229*^9}},
CellLabel->"In[1]:=",ExpressionUUID->"bb7c7b01-0cfa-4c90-a2db-09590ed14465"],
Cell[BoxData["\<\"/home/alex/Programming/esp/uas-ugv\"\>"], "Output",
CellChangeTimes->{3.764181137613065*^9},
CellLabel->"Out[1]=",ExpressionUUID->"d714e440-83e1-402e-946a-6ad01a10cf86"]
}, Open ]],
Cell[BoxData[{
RowBox[{
RowBox[{"data", "=",
RowBox[{"Import", "[",
RowBox[{"\"\<UGVDATA_2019-04-13 14-43-56.csv\>\"", ",",
RowBox[{"HeaderLines", "\[Rule]", "1"}]}], "]"}]}],
";"}], "\[IndentingNewLine]",
RowBox[{
RowBox[{"magdata", "=",
RowBox[{"data", "[",
RowBox[{"[",
RowBox[{"All", ",",
RowBox[{"7", ";;", "9"}]}], "]"}], "]"}]}], ";"}]}], "Input",
CellChangeTimes->{{3.764181138593279*^9, 3.764181180255095*^9}, {
3.7641814517144814`*^9, 3.764181455934593*^9}, {3.764181490536357*^9,
3.764181494963828*^9}, {3.764181657781171*^9, 3.7641816705271997`*^9}},
CellLabel->"In[28]:=",ExpressionUUID->"9d982e0c-62ac-49c9-8fab-0efac1e57ee1"],
Cell[BoxData[
RowBox[{
RowBox[{"plotdata", "[", "data_", "]"}], ":=",
RowBox[{"ListPlot", "[",
RowBox[{
RowBox[{"{",
RowBox[{
RowBox[{"data", "[",
RowBox[{"[",
RowBox[{"All", ",",
RowBox[{"1", ";;", "2"}]}], "]"}], "]"}], ",",
RowBox[{"data", "[",
RowBox[{"[",
RowBox[{"All", ",",
RowBox[{"2", ";;", "3"}]}], "]"}], "]"}], ",",
RowBox[{"Transpose", "[",
RowBox[{"{",
RowBox[{
RowBox[{"data", "[",
RowBox[{"[",
RowBox[{"All", ",", "3"}], "]"}], "]"}], ",",
RowBox[{"data", "[",
RowBox[{"[",
RowBox[{"All", ",", "1"}], "]"}], "]"}]}], "}"}], "]"}]}], "}"}],
",",
RowBox[{"AspectRatio", "\[Rule]", "1"}]}], "]"}]}]], "Input",
CellChangeTimes->{{3.764181183734476*^9, 3.764181194453432*^9}, {
3.7641812278125753`*^9, 3.764181258624784*^9}, {3.764181292610927*^9,
3.764181311588319*^9}, {3.764181372869302*^9, 3.764181404947816*^9}, {
3.764181593568303*^9, 3.764181595326483*^9}, {3.764181673887734*^9,
3.764181742499264*^9}},
CellLabel->"In[30]:=",ExpressionUUID->"9e6d0196-dead-4dc5-9762-497e415abc3e"],
Cell[CellGroupData[{
Cell[BoxData[
RowBox[{"plotdata", "[", "magdata", "]"}]], "Input",
CellChangeTimes->{{3.764181745429572*^9, 3.7641817495884113`*^9}},
CellLabel->"In[31]:=",ExpressionUUID->"55fc2693-bfc0-4284-878d-723d16de613e"],
Cell[BoxData[
GraphicsBox[{{}, {{},
{RGBColor[0.368417, 0.506779, 0.709798], PointSize[0.004583333333333334],
AbsoluteThickness[1.6], PointBox[CompressedData["
1:eJyl2XtQVNcZAHCC1geLr/hYRXBZWOVV3L0syGOBPewKRJwkNVqQWGoUTImP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"]]},
{RGBColor[0.880722, 0.611041, 0.142051], PointSize[0.004583333333333334],
AbsoluteThickness[1.6], PointBox[CompressedData["
1:eJyl2XlQVEcaAHBUEhXIQURHwQGG4VBRmWFQjhGnEZ0RcHSIBgQ1HkCiaMpr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"]]},
{RGBColor[0.560181, 0.691569, 0.194885], PointSize[0.004583333333333334],
AbsoluteThickness[1.6], PointBox[CompressedData["
1:eJyt2XlQFFcaAHBUonJERVEUgQFmAAWRGUAQAacBmRFwcIgGBCVGgY3nonEj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"]]}, {}}, {}, {}, {}, {}},
AspectRatio->1,
Axes->{True, True},
AxesLabel->{None, None},
AxesOrigin->{0, 0},
DisplayFunction->Identity,
Frame->{{False, False}, {False, False}},
FrameLabel->{{None, None}, {None, None}},
FrameTicks->{{Automatic, Automatic}, {Automatic, Automatic}},
GridLines->{None, None},
GridLinesStyle->Directive[
GrayLevel[0.5, 0.4]],
ImagePadding->All,
Method->{"CoordinatesToolOptions" -> {"DisplayFunction" -> ({
(Identity[#]& )[
Part[#, 1]],
(Identity[#]& )[
Part[#, 2]]}& ), "CopiedValueFunction" -> ({
(Identity[#]& )[
Part[#, 1]],
(Identity[#]& )[
Part[#, 2]]}& )}},
PlotRange->{{-70.621246, 43.332359}, {-70.621246, 43.332359}},
PlotRangeClipping->True,
PlotRangePadding->{{
Scaled[0.02],
Scaled[0.02]}, {
Scaled[0.05],
Scaled[0.05]}},
Ticks->{Automatic, Automatic}]], "Output",
CellChangeTimes->{3.764181749939802*^9},
CellLabel->"Out[31]=",ExpressionUUID->"902da2ce-607d-4066-90c0-08d837866834"]
}, Open ]],
Cell[CellGroupData[{
Cell[BoxData[
RowBox[{"magavgs", "=",
RowBox[{"Mean", "[", "magdata", "]"}]}]], "Input",
CellChangeTimes->{{3.764181409889127*^9, 3.764181442609482*^9}, {
3.764181514100668*^9, 3.7641815148825817`*^9}, {3.764181607119375*^9,
3.764181607403371*^9}, {3.764181758111438*^9, 3.764181767887418*^9}},
CellLabel->"In[32]:=",ExpressionUUID->"f29f83fe-0d9c-4ec9-a48d-4ac4733e0b37"],
Cell[BoxData[
RowBox[{"{",
RowBox[{"3.269470381679388`", ",",
RowBox[{"-", "1.1777647652671779`"}], ",",
RowBox[{"-", "27.38331706488549`"}]}], "}"}]], "Output",
CellChangeTimes->{{3.7641814300122833`*^9, 3.764181460376156*^9}, {
3.7641814985945263`*^9, 3.764181515876246*^9}, 3.764181791217758*^9},
CellLabel->"Out[32]=",ExpressionUUID->"b2979ec0-2228-4101-a551-ceafaf22282f"]
}, Open ]],
Cell[CellGroupData[{
Cell[BoxData[
RowBox[{"magmeds", "=",
RowBox[{"Median", "[", "magdata", "]"}]}]], "Input",
CellChangeTimes->{{3.764181613250167*^9, 3.764181618795424*^9}, {
3.7641817603848677`*^9, 3.764181772489642*^9}},
CellLabel->"In[33]:=",ExpressionUUID->"9c3fe212-89fa-46ec-bed5-0affe4811ced"],
Cell[BoxData[
RowBox[{"{",
RowBox[{
RowBox[{"-", "1.49939`"}], ",", "0.749695`", ",",
RowBox[{"-", "21.141394`"}]}], "}"}]], "Output",
CellChangeTimes->{3.764181619514329*^9, 3.764181793290881*^9},
CellLabel->"Out[33]=",ExpressionUUID->"ef485533-27f2-4451-90fc-eff443044e16"]
}, Open ]],
Cell[CellGroupData[{
Cell[BoxData[{
RowBox[{
RowBox[{"magdata2", "=",
RowBox[{
RowBox[{
RowBox[{"#", "-", "magavgs"}], "&"}], "/@", "magdata"}]}],
";"}], "\[IndentingNewLine]",
RowBox[{"plotdata", "[", "magdata2", "]"}]}], "Input",
CellChangeTimes->{{3.764181501987578*^9, 3.764181558842853*^9}, {
3.764181623169281*^9, 3.764181639417089*^9}, {3.764181762202607*^9,
3.7641818300440407`*^9}},
CellLabel->"In[42]:=",ExpressionUUID->"bd9700b2-e7a8-4e50-89dd-94e26b677ce3"],
Cell[BoxData[
GraphicsBox[{{}, {{},
{RGBColor[0.368417, 0.506779, 0.709798], PointSize[0.004583333333333334],
AbsoluteThickness[1.6], PointBox[CompressedData["
1:eJyl2Wk4lGsfAHDLhFJZT2HGMpZhBoOZMWPGMLcl2lQUSTqVVJQlqhM5JaF0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"]]},
{RGBColor[0.880722, 0.611041, 0.142051], PointSize[0.004583333333333334],
AbsoluteThickness[1.6], PointBox[CompressedData["
1:eJyl2Xk01GsfAPDJEi7eSMKEsQ3GOmY1Y5ZnkKWiKEulsr5JZYsS3ZIk5Vba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"]]},
{RGBColor[0.560181, 0.691569, 0.194885], PointSize[0.004583333333333334],
AbsoluteThickness[1.6], PointBox[CompressedData["
1:eJyl2Xk0lOsfAHDJtYTsWTO2GcYyZoYZxmzPzFhvKRVCJURStqRfpLSSrpKW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"]]}, {}}, {}, {}, {}, {}},
AspectRatio->1,
Axes->{True, True},
AxesLabel->{None, None},
AxesOrigin->{0, 0},
DisplayFunction->Identity,
Frame->{{False, False}, {False, False}},
FrameLabel->{{None, None}, {None, None}},
FrameTicks->{{Automatic, Automatic}, {Automatic, Automatic}},
GridLines->{None, None},
GridLinesStyle->Directive[
GrayLevel[0.5, 0.4]],
ImagePadding->All,
Method->{"CoordinatesToolOptions" -> {"DisplayFunction" -> ({
(Identity[#]& )[
Part[#, 1]],
(Identity[#]& )[
Part[#, 2]]}& ), "CopiedValueFunction" -> ({
(Identity[#]& )[
Part[#, 1]],
(Identity[#]& )[
Part[#, 2]]}& )}},
PlotRange->{{-43.23792893511451, 40.062888618320606`}, {-43.23792893511451,
40.062888618320606`}},
PlotRangeClipping->True,
PlotRangePadding->{{
Scaled[0.02],
Scaled[0.02]}, {
Scaled[0.05],
Scaled[0.05]}},
Ticks->{Automatic, Automatic}]], "Output",
CellChangeTimes->{{3.764181801868524*^9, 3.764181830670013*^9}},
CellLabel->"Out[43]=",ExpressionUUID->"95e60565-77ed-4935-ba0d-34c22a07a34b"]
}, Open ]],
Cell[CellGroupData[{
Cell[BoxData[{
RowBox[{
RowBox[{"Norm", "/@", "magdata2"}], ";"}], "\[IndentingNewLine]",
RowBox[{"Histogram", "[", "%", "]"}]}], "Input",
CellChangeTimes->{{3.764181851587598*^9, 3.764181901887018*^9}},
CellLabel->"In[48]:=",ExpressionUUID->"6e2382e3-be00-4738-b380-b67947d32194"],
Cell[BoxData[
GraphicsBox[{
{RGBColor[0.987148, 0.8073604000000001, 0.49470040000000004`], EdgeForm[{
Opacity[0.609], Thickness[Small]}], {},
{RGBColor[0.987148, 0.8073604000000001, 0.49470040000000004`], EdgeForm[{
Opacity[0.609], Thickness[Small]}],
TagBox[
TooltipBox[
TagBox[
DynamicBox[{
FEPrivate`If[
CurrentValue["MouseOver"],
EdgeForm[{
GrayLevel[0.5],
AbsoluteThickness[1.5],
Opacity[0.66]}], {}, {}],
RectangleBox[{24., 0}, {26., 3.}, "RoundingRadius" -> 0]},
ImageSizeCache->{{21.350480384307446`, 51.85266394934132}, {
85.14712719107074, 92.96535391600986}}],
StatusArea[#, 3]& ,
TagBoxNote->"3"],
StyleBox["3", {}, StripOnInput -> False]],
Annotation[#,
Style[3, {}], "Tooltip"]& ],
TagBox[
TooltipBox[
TagBox[
DynamicBox[{
FEPrivate`If[
CurrentValue["MouseOver"],
EdgeForm[{
GrayLevel[0.5],
AbsoluteThickness[1.5],
Opacity[0.66]}], {}, {}],
RectangleBox[{26., 0}, {28., 44.}, "RoundingRadius" -> 0]},
ImageSizeCache->{{51.35266394934132,
81.85484751437514}, {-14.86863804976393, 92.96535391600986}}],
StatusArea[#, 44]& ,
TagBoxNote->"44"],
StyleBox["44", {}, StripOnInput -> False]],
Annotation[#,
Style[44, {}], "Tooltip"]& ],
TagBox[
TooltipBox[
TagBox[
DynamicBox[{
FEPrivate`If[
CurrentValue["MouseOver"],
EdgeForm[{
GrayLevel[0.5],
AbsoluteThickness[1.5],
Opacity[0.66]}], {}, {}],
RectangleBox[{28., 0}, {30., 59.}, "RoundingRadius" -> 0]},
ImageSizeCache->{{81.35484751437514,
111.85703107940901`}, {-51.459771674459546`, 92.96535391600986}}],
StatusArea[#, 59]& ,
TagBoxNote->"59"],
StyleBox["59", {}, StripOnInput -> False]],
Annotation[#,
Style[59, {}], "Tooltip"]& ],
TagBox[
TooltipBox[
TagBox[
DynamicBox[{
FEPrivate`If[
CurrentValue["MouseOver"],
EdgeForm[{
GrayLevel[0.5],
AbsoluteThickness[1.5],
Opacity[0.66]}], {}, {}],
RectangleBox[{30., 0}, {32., 70.}, "RoundingRadius" -> 0]},
ImageSizeCache->{{111.35703107940901`,
141.85921464444283`}, {-78.29326966590298, 92.96535391600986}}],
StatusArea[#, 70]& ,
TagBoxNote->"70"],
StyleBox["70", {}, StripOnInput -> False]],
Annotation[#,
Style[70, {}], "Tooltip"]& ],
TagBox[
TooltipBox[
TagBox[
DynamicBox[{
FEPrivate`If[
CurrentValue["MouseOver"],
EdgeForm[{
GrayLevel[0.5],
AbsoluteThickness[1.5],
Opacity[0.66]}], {}, {}],
RectangleBox[{32., 0}, {34., 57.}, "RoundingRadius" -> 0]},
ImageSizeCache->{{141.35921464444283`,
171.86139820947665`}, {-46.580953857833464`, 92.96535391600986}}],
StatusArea[#, 57]& ,
TagBoxNote->"57"],
StyleBox["57", {}, StripOnInput -> False]],
Annotation[#,
Style[57, {}], "Tooltip"]& ],
TagBox[
TooltipBox[
TagBox[
DynamicBox[{
FEPrivate`If[
CurrentValue["MouseOver"],
EdgeForm[{
GrayLevel[0.5],
AbsoluteThickness[1.5],
Opacity[0.66]}], {}, {}],
RectangleBox[{34., 0}, {36., 81.}, "RoundingRadius" -> 0]},
ImageSizeCache->{{171.36139820947665`,
201.86358177451052`}, {-105.12676765734642`, 92.96535391600986}}],
StatusArea[#, 81]& ,
TagBoxNote->"81"],
StyleBox["81", {}, StripOnInput -> False]],
Annotation[#,
Style[81, {}], "Tooltip"]& ],
TagBox[
TooltipBox[
TagBox[
DynamicBox[{
FEPrivate`If[
CurrentValue["MouseOver"],
EdgeForm[{
GrayLevel[0.5],
AbsoluteThickness[1.5],
Opacity[0.66]}], {}, {}],
RectangleBox[{36., 0}, {38., 62.}, "RoundingRadius" -> 0]},
ImageSizeCache->{{201.36358177451052`,
231.8657653395444}, {-58.777998399398655`, 92.96535391600986}}],
StatusArea[#, 62]& ,
TagBoxNote->"62"],
StyleBox["62", {}, StripOnInput -> False]],
Annotation[#,
Style[62, {}], "Tooltip"]& ],
TagBox[
TooltipBox[
TagBox[
DynamicBox[{
FEPrivate`If[
CurrentValue["MouseOver"],
EdgeForm[{
GrayLevel[0.5],
AbsoluteThickness[1.5],
Opacity[0.66]}], {}, {}],
RectangleBox[{38., 0}, {40., 80.}, "RoundingRadius" -> 0]},
ImageSizeCache->{{231.3657653395444,
261.86794890457816`}, {-102.6873587490334, 92.96535391600986}}],
StatusArea[#, 80]& ,
TagBoxNote->"80"],
StyleBox["80", {}, StripOnInput -> False]],
Annotation[#,
Style[80, {}], "Tooltip"]& ],
TagBox[
TooltipBox[
TagBox[
DynamicBox[{
FEPrivate`If[
CurrentValue["MouseOver"],
EdgeForm[{
GrayLevel[0.5],
AbsoluteThickness[1.5],
Opacity[0.66]}], {}, {}],
RectangleBox[{40., 0}, {42., 40.}, "RoundingRadius" -> 0]},
ImageSizeCache->{{261.36794890457816`,
291.87013246961203`}, {-5.1110024165117665`, 92.96535391600986}}],
StatusArea[#, 40]& ,
TagBoxNote->"40"],
StyleBox["40", {}, StripOnInput -> False]],
Annotation[#,
Style[40, {}], "Tooltip"]& ],
TagBox[
TooltipBox[
TagBox[
DynamicBox[{
FEPrivate`If[
CurrentValue["MouseOver"],
EdgeForm[{
GrayLevel[0.5],
AbsoluteThickness[1.5],
Opacity[0.66]}], {}, {}],
RectangleBox[{42., 0}, {44., 20.}, "RoundingRadius" -> 0]},
ImageSizeCache->{{291.37013246961203`, 321.8723160346459}, {
43.67717574974905, 92.96535391600986}}],
StatusArea[#, 20]& ,
TagBoxNote->"20"],
StyleBox["20", {}, StripOnInput -> False]],
Annotation[#,
Style[20, {}], "Tooltip"]& ],
TagBox[
TooltipBox[
TagBox[
DynamicBox[{
FEPrivate`If[
CurrentValue["MouseOver"],
EdgeForm[{
GrayLevel[0.5],
AbsoluteThickness[1.5],
Opacity[0.66]}], {}, {}],
RectangleBox[{44., 0}, {46., 8.}, "RoundingRadius" -> 0]},
ImageSizeCache->{{321.3723160346459, 351.8744995996798}, {
72.95008264950553, 92.96535391600986}}],
StatusArea[#, 8]& ,
TagBoxNote->"8"],
StyleBox["8", {}, StripOnInput -> False]],
Annotation[#,
Style[8, {}],
"Tooltip"]& ]}, {}, {}}, {{}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}}},
AspectRatio->NCache[GoldenRatio^(-1), 0.6180339887498948],
Axes->{True, True},
AxesLabel->{None, None},
AxesOrigin->{23.56, 0},
FrameLabel->{{None, None}, {None, None}},
FrameTicks->{{Automatic, Automatic}, {Automatic, Automatic}},
GridLines->{None, None},
GridLinesStyle->Directive[
GrayLevel[0.5, 0.4]],
PlotRange->{{24., 46.}, {All, All}},
PlotRangePadding->{{
Scaled[0.02],
Scaled[0.02]}, {
Scaled[0.02],
Scaled[0.05]}},
Ticks->{Automatic, Automatic}]], "Output",
CellChangeTimes->{{3.764181855648719*^9, 3.764181902239205*^9}},
CellLabel->"Out[49]=",ExpressionUUID->"23ec7069-7289-4ea8-840d-63776a63d3eb"]
}, Open ]]
},
WindowSize->{808, 911},
WindowMargins->{{Automatic, 634}, {118, Automatic}},
FrontEndVersion->"11.3 for Linux x86 (64-bit) (March 6, 2018)",
StyleDefinitions->"Default.nb"
]
(* End of Notebook Content *)
(* Internal cache information *)
(*CellTagsOutline
CellTagsIndex->{}
*)
(*CellTagsIndex
CellTagsIndex->{}
*)
(*NotebookFileOutline
Notebook[{
Cell[CellGroupData[{
Cell[580, 22, 253, 4, 31, "Input",ExpressionUUID->"bb7c7b01-0cfa-4c90-a2db-09590ed14465"],
Cell[836, 28, 189, 2, 35, "Output",ExpressionUUID->"d714e440-83e1-402e-946a-6ad01a10cf86"]
}, Open ]],
Cell[1040, 33, 694, 16, 55, "Input",ExpressionUUID->"9d982e0c-62ac-49c9-8fab-0efac1e57ee1"],
Cell[1737, 51, 1197, 31, 78, "Input",ExpressionUUID->"9e6d0196-dead-4dc5-9762-497e415abc3e"],
Cell[CellGroupData[{
Cell[2959, 86, 214, 3, 31, "Input",ExpressionUUID->"55fc2693-bfc0-4284-878d-723d16de613e"],
Cell[3176, 91, 10583, 191, 377, "Output",ExpressionUUID->"902da2ce-607d-4066-90c0-08d837866834"]
}, Open ]],
Cell[CellGroupData[{
Cell[13796, 287, 383, 6, 31, "Input",ExpressionUUID->"f29f83fe-0d9c-4ec9-a48d-4ac4733e0b37"],
Cell[14182, 295, 394, 7, 35, "Output",ExpressionUUID->"b2979ec0-2228-4101-a551-ceafaf22282f"]
}, Open ]],
Cell[CellGroupData[{
Cell[14613, 307, 290, 5, 31, "Input",ExpressionUUID->"9c3fe212-89fa-46ec-bed5-0affe4811ced"],
Cell[14906, 314, 288, 6, 35, "Output",ExpressionUUID->"ef485533-27f2-4451-90fc-eff443044e16"]
}, Open ]],
Cell[CellGroupData[{
Cell[15231, 325, 476, 11, 55, "Input",ExpressionUUID->"bd9700b2-e7a8-4e50-89dd-94e26b677ce3"],
Cell[15710, 338, 13359, 236, 377, "Output",ExpressionUUID->"95e60565-77ed-4935-ba0d-34c22a07a34b"]
}, Open ]],
Cell[CellGroupData[{
Cell[29106, 579, 288, 5, 55, "Input",ExpressionUUID->"6e2382e3-be00-4738-b380-b67947d32194"],
Cell[29397, 586, 7797, 223, 243, "Output",ExpressionUUID->"23ec7069-7289-4ea8-840d-63776a63d3eb"]
}, Open ]]
}
]
*)
(* End of internal cache information *)

19
calib/calib.py

@ -3,9 +3,9 @@ import serial @@ -3,9 +3,9 @@ import serial
import re
import datetime
if __name__ == "__main__":
def capture_data():
num_re = r"([\-0-9\.]+)"
line_re = re.compile(r"inputs: acc=\({0}, {0}, {0}\) gyro=\({0}, {0}, {0}\) mag=\({0}, {0}, {0}\)".format(num_re))
line_re = re.compile(r".*inputs: acc=\({0}, {0}, {0}\) gyro=\({0}, {0}, {0}\) mag=\({0}, {0}, {0}\)".format(num_re))
ser = serial.serial_for_url("hwgrep://", baudrate=115200, parity=serial.PARITY_NONE,
stopbits=serial.STOPBITS_ONE, bytesize=serial.EIGHTBITS,
@ -14,14 +14,27 @@ if __name__ == "__main__": @@ -14,14 +14,27 @@ if __name__ == "__main__":
timestr = datetime.datetime.now().strftime("%Y-%m-%d %H-%M-%S")
fname = "UGVDATA_{}.csv".format(timestr)
with open(fname, "w") as f:
try:
f.write("AX,AY,AZ,GX,GY,GZ,MX,MY,MZ\n")
while True:
line = ser.read_until()
try:
line = ser.read_until().decode("utf-8")
except Exception as e:
print("line decode error: ", e)
continue
matches = line_re.match(line)
if not matches:
print("line did not match: ", line)
continue
nums = [str(numstr) for numstr in matches.groups()]
if len(nums) != 9:
continue
f.write(",".join(nums))
f.write("\n")
except KeyboardInterrupt:
print("interrupt")
f.flush()
return
if __name__ == "__main__":
capture_data()

12
calib/plot.sh

@ -0,0 +1,12 @@ @@ -0,0 +1,12 @@
#!/usr/bin/env bash
file=$1
gnuplot -persist <<EOF
set datafile separator ","
set xzeroaxis
set yzeroaxis
set style data points
set pointsize 0.5
plot "$file" using 7:8 with points title "X-Y", "$file" using 8:9 with points title "Y-Z", "$file" using 9:7 with points title "Z-X"
EOF

47
main/ugv_io_mpu.cc

@ -22,8 +22,9 @@ static constexpr float MPU_MAG_TO_FLUX = (4912.f) / (32760.f); @@ -22,8 +22,9 @@ static constexpr float MPU_MAG_TO_FLUX = (4912.f) / (32760.f);
static const Vec3f ACCEL_OFFSET = {0., 0., 0.};
static const Mat3f ACCEL_MAT = {1., 0., 0., 0., 1., 0., 0., 0., 1.};
static const Vec3f MAG_OFFSET = {0., 0., 0.};
static const Mat3f MAG_MAT = {1., 0., 0., 0., 1., 0., 0., 0., 1.};
static const Vec3f MAG_OFFSET = {-7.79683, 3.6735, 32.3868};
static const Mat3f MAG_MAT = {0., -0.0281408, 0., -0.0284409, 0., 0., 0., 0., 0.0261544};
static const Mat3f GYRO_MAT = {1., 0., 0., 0., 1., 0., 0., 0., 1.};
static const Vec3f GYRO_OFFSET = {0., 0., 0.};
static const char *TAG = "ugv_io_mpu";
@ -39,26 +40,36 @@ MPU::~MPU() { delete mpu_; } @@ -39,26 +40,36 @@ MPU::~MPU() { delete mpu_; }
void MPU::Init() {
xSemaphoreTake(i2c_mutex, portMAX_DELAY);
mpu_bus_ = &i2c0;
mpu_bus_ = &i2c1;
// This is shared with the oled, so just use those pins
mpu_bus_->setTimeout(10);
mpu_bus_->begin(MPU_SDA, MPU_SCL, 100000);
mpu_ = new mpud::MPU(*mpu_bus_);
esp_err_t ret;
int tries = 0;
for (; tries < 5; ++tries) {
mpu_->getInterruptStatus();
ret = mpu_->testConnection();
if (ret != ESP_OK) {
uint8_t wai = mpu_->whoAmI();
ESP_LOGE(TAG, "MPU not connected (whoAmI: %d)", wai);
return;
ESP_LOGE(TAG, "MPU not connected (whoAmI: %#x)", wai);
vTaskDelay(pdMS_TO_TICKS(100));
continue;
}
ret = mpu_->compassTestConnection();
if (ret != ESP_OK) {
uint8_t wai = mpu_->compassWhoAmI();
ESP_LOGW(TAG, "MPU compass not connected (whoAmI: %d)", wai);
ESP_LOGW(TAG, "MPU compass not connected (whoAmI: %#x)", wai);
}
ret = mpu_->initialize();
if (ret != ESP_OK) {
ESP_LOGE(TAG, "MPU initialization error");
continue;
}
break;
}
if (tries == 5) {
return;
}
// Calibrate();
@ -86,27 +97,19 @@ void MPU::Calibrate() { @@ -86,27 +97,19 @@ void MPU::Calibrate() {
void MPU::GetData(MpuData &data) {
esp_err_t ret;
// uint8_t mxh, mxl, myh, myl, mzh, mzl, n;
xSemaphoreTake(i2c_mutex, portMAX_DELAY);
ret = mpu_->motion(&accel_, &gyro_);
if (ret != ESP_OK) {
ESP_LOGE(TAG, "error reading MPU: %#x", ret);
}
uint8_t compass_data[7];
mpu_->setAuxI2CBypass(true);
mpu_bus_->readBytes(mpud::COMPASS_I2CADDRESS, 0x03, 7, compass_data);
mpu_->setAuxI2CBypass(false);
// mpu_->compassReadByte(0x03, &mxl);
// mpu_->compassReadByte(0x04, &mxh);
// mpu_->compassReadByte(0x05, &myl);
// mpu_->compassReadByte(0x06, &myh);
// mpu_->compassReadByte(0x07, &mzl);
// mpu_->compassReadByte(0x08, &mzh);
// mpu_->compassReadByte(0x09, &n);
xSemaphoreGive(i2c_mutex);
ret = mpu_bus_->readBytes(mpud::COMPASS_I2CADDRESS, 0x03, 7, compass_data);
if (ret != ESP_OK) {
ESP_LOGE(TAG, "error reading MPU");
ESP_LOGE(TAG, "error reading MPU compass: %#x", ret);
}
// int16_t mx = (static_cast<int16_t>(mxh) << 8) | static_cast<int16_t>(mxl);
// int16_t my = (static_cast<int16_t>(myh) << 8) | static_cast<int16_t>(myl);
// int16_t mz = (static_cast<int16_t>(mzh) << 8) | static_cast<int16_t>(mzl);
mpu_->setAuxI2CBypass(false);
xSemaphoreGive(i2c_mutex);
int16_t mx = (static_cast<int16_t>(compass_data[1]) << 8) |
static_cast<int16_t>(compass_data[0]);
int16_t my = (static_cast<int16_t>(compass_data[3]) << 8) |
@ -117,7 +120,7 @@ void MPU::GetData(MpuData &data) { @@ -117,7 +120,7 @@ void MPU::GetData(MpuData &data) {
data.accel = mpud::accelGravity(accel_, MPU_ACCEL_FS);
data.accel = ACCEL_MAT * (data.accel + ACCEL_OFFSET);
data.gyro_rate = mpud::gyroDegPerSec(gyro_, MPU_GYRO_FS);
data.gyro_rate += GYRO_OFFSET;
data.gyro_rate = GYRO_MAT * (data.gyro_rate + GYRO_OFFSET);
data.mag.x = ((float)mx) * MPU_MAG_TO_FLUX;
data.mag.y = ((float)my) * MPU_MAG_TO_FLUX;
data.mag.z = ((float)mz) * MPU_MAG_TO_FLUX;

2
main/ugv_io_mpu.hh

@ -31,7 +31,7 @@ struct Mat3f { @@ -31,7 +31,7 @@ struct Mat3f {
float zx, zy, zz;
Vec3f operator*(const Vec3f& v) const {
return {xx * v.x + xy * v.y + xz + v.z,
return {xx * v.x + xy * v.y + xz * v.z,
yx * v.x + yy * v.y + yz * v.z,
zx * v.x + zy * v.y + zz * v.z};
}

Loading…
Cancel
Save