Changeset 15 in flair-src for trunk/lib/FlairFilter/src/Ahrs_impl.cpp
- Timestamp:
- Apr 8, 2016, 3:40:57 PM (9 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/lib/FlairFilter/src/Ahrs_impl.cpp
r10 r15 30 30 using namespace flair::sensor; 31 31 32 Ahrs_impl::Ahrs_impl(Ahrs * inSelf): self(inSelf) {33 rollPlot=NULL;34 pitchPlot=NULL;35 yawPlot=NULL;36 wXPlot=NULL;37 wYPlot=NULL;38 wZPlot=NULL;39 q0Plot=NULL;40 q1Plot=NULL;41 q2Plot=NULL;42 q3Plot=NULL;32 Ahrs_impl::Ahrs_impl(Ahrs *inSelf) : self(inSelf) { 33 rollPlot = NULL; 34 pitchPlot = NULL; 35 yawPlot = NULL; 36 wXPlot = NULL; 37 wYPlot = NULL; 38 wZPlot = NULL; 39 q0Plot = NULL; 40 q1Plot = NULL; 41 q2Plot = NULL; 42 q3Plot = NULL; 43 43 44 eulerTab=NULL;45 quaternionTab=NULL;44 eulerTab = NULL; 45 quaternionTab = NULL; 46 46 47 ahrsData=new AhrsData(self);47 ahrsData = new AhrsData(self); 48 48 } 49 49 50 Ahrs_impl::~Ahrs_impl() { 51 } 50 Ahrs_impl::~Ahrs_impl() {} 52 51 53 52 void Ahrs_impl::UseDefaultPlot(void) { 54 53 55 eulerTab=new Tab(((Imu*)(self->Parent()))->tab,"AHRS");56 rollPlot=new DataPlot1D(eulerTab->NewRow(),"roll",-30,30);57 58 pitchPlot=new DataPlot1D(eulerTab->LastRowLastCol(),"pitch",-30,30);59 60 yawPlot=new DataPlot1D(eulerTab->LastRowLastCol(),"yaw",-180,180);61 54 eulerTab = new Tab(((Imu *)(self->Parent()))->tab, "AHRS"); 55 rollPlot = new DataPlot1D(eulerTab->NewRow(), "roll", -30, 30); 56 rollPlot->AddCurve(ahrsData->Element(AhrsData::RollDeg)); 57 pitchPlot = new DataPlot1D(eulerTab->LastRowLastCol(), "pitch", -30, 30); 58 pitchPlot->AddCurve(ahrsData->Element(AhrsData::PitchDeg)); 59 yawPlot = new DataPlot1D(eulerTab->LastRowLastCol(), "yaw", -180, 180); 60 yawPlot->AddCurve(ahrsData->Element(AhrsData::YawDeg)); 62 61 63 wXPlot=new DataPlot1D(eulerTab->NewRow(),"w_x",-200,200);64 65 wYPlot=new DataPlot1D(eulerTab->LastRowLastCol(),"w_y",-200,200);66 67 wZPlot=new DataPlot1D(eulerTab->LastRowLastCol(),"w_z",-200,200);68 62 wXPlot = new DataPlot1D(eulerTab->NewRow(), "w_x", -200, 200); 63 wXPlot->AddCurve(ahrsData->Element(AhrsData::WxDeg)); 64 wYPlot = new DataPlot1D(eulerTab->LastRowLastCol(), "w_y", -200, 200); 65 wYPlot->AddCurve(ahrsData->Element(AhrsData::WyDeg)); 66 wZPlot = new DataPlot1D(eulerTab->LastRowLastCol(), "w_z", -200, 200); 67 wZPlot->AddCurve(ahrsData->Element(AhrsData::WzDeg)); 69 68 70 quaternionTab=new Tab(((Imu*)(self->Parent()))->tab,"Quaternion"); 71 q0Plot=new DataPlot1D(quaternionTab->NewRow(),"q0",-1,1); 72 q0Plot->AddCurve(ahrsData->Element(AhrsData::Q0)); 73 q1Plot=new DataPlot1D(quaternionTab->NewRow(),"q1",-1,1); 74 q1Plot->AddCurve(ahrsData->Element(AhrsData::Q1)); 75 q2Plot=new DataPlot1D(quaternionTab->LastRowLastCol(),"q2",-1,1); 76 q2Plot->AddCurve(ahrsData->Element(AhrsData::Q2)); 77 q3Plot=new DataPlot1D(quaternionTab->LastRowLastCol(),"q3",-1,1); 78 q3Plot->AddCurve(ahrsData->Element(AhrsData::Q3)); 79 69 quaternionTab = new Tab(((Imu *)(self->Parent()))->tab, "Quaternion"); 70 q0Plot = new DataPlot1D(quaternionTab->NewRow(), "q0", -1, 1); 71 q0Plot->AddCurve(ahrsData->Element(AhrsData::Q0)); 72 q1Plot = new DataPlot1D(quaternionTab->NewRow(), "q1", -1, 1); 73 q1Plot->AddCurve(ahrsData->Element(AhrsData::Q1)); 74 q2Plot = new DataPlot1D(quaternionTab->LastRowLastCol(), "q2", -1, 1); 75 q2Plot->AddCurve(ahrsData->Element(AhrsData::Q2)); 76 q3Plot = new DataPlot1D(quaternionTab->LastRowLastCol(), "q3", -1, 1); 77 q3Plot->AddCurve(ahrsData->Element(AhrsData::Q3)); 80 78 } 81 79 82 void Ahrs_impl::AddPlot(const AhrsData *ahrsData, DataPlot::Color_t color) {83 if(rollPlot!=NULL) {84 rollPlot->AddCurve(ahrsData->Element(AhrsData::RollDeg),color);85 pitchPlot->AddCurve(ahrsData->Element(AhrsData::PitchDeg),color);86 yawPlot->AddCurve(ahrsData->Element(AhrsData::YawDeg),color);87 88 if(wXPlot!=NULL) {89 wXPlot->AddCurve(ahrsData->Element(AhrsData::WxDeg),color);90 wYPlot->AddCurve(ahrsData->Element(AhrsData::WyDeg),color);91 wZPlot->AddCurve(ahrsData->Element(AhrsData::WzDeg),color);92 93 if(quaternionTab!=NULL) {94 q0Plot->AddCurve(ahrsData->Element(AhrsData::Q0),color);95 q1Plot->AddCurve(ahrsData->Element(AhrsData::Q1),color);96 q2Plot->AddCurve(ahrsData->Element(AhrsData::Q2),color);97 q3Plot->AddCurve(ahrsData->Element(AhrsData::Q3),color);98 80 void Ahrs_impl::AddPlot(const AhrsData *ahrsData, DataPlot::Color_t color) { 81 if (rollPlot != NULL) { 82 rollPlot->AddCurve(ahrsData->Element(AhrsData::RollDeg), color); 83 pitchPlot->AddCurve(ahrsData->Element(AhrsData::PitchDeg), color); 84 yawPlot->AddCurve(ahrsData->Element(AhrsData::YawDeg), color); 85 } 86 if (wXPlot != NULL) { 87 wXPlot->AddCurve(ahrsData->Element(AhrsData::WxDeg), color); 88 wYPlot->AddCurve(ahrsData->Element(AhrsData::WyDeg), color); 89 wZPlot->AddCurve(ahrsData->Element(AhrsData::WzDeg), color); 90 } 91 if (quaternionTab != NULL) { 92 q0Plot->AddCurve(ahrsData->Element(AhrsData::Q0), color); 93 q1Plot->AddCurve(ahrsData->Element(AhrsData::Q1), color); 94 q2Plot->AddCurve(ahrsData->Element(AhrsData::Q2), color); 95 q3Plot->AddCurve(ahrsData->Element(AhrsData::Q3), color); 96 } 99 97 }
Note:
See TracChangeset
for help on using the changeset viewer.