Changeset 167 in flair-src for trunk/demos/CircleFollower/uav
- Timestamp:
- Apr 12, 2017, 1:59:38 PM (7 years ago)
- Location:
- trunk/demos/CircleFollower/uav/src
- Files:
-
- 2 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/demos/CircleFollower/uav/src/CircleFollower.cpp
r147 r167 89 89 //get roll, pitch and w from imu 90 90 Quaternion ahrsQuaternion; 91 Vector3D ahrsAngularSpeed;91 Vector3Df ahrsAngularSpeed; 92 92 GetDefaultOrientation()->GetQuaternionAndAngularRates(ahrsQuaternion, ahrsAngularSpeed); 93 93 … … 101 101 } 102 102 103 void CircleFollower::AltitudeValues(float &z,float &dz) {104 Vector3D uav_pos,uav_vel;103 void CircleFollower::AltitudeValues(float &z,float &dz) const{ 104 Vector3Df uav_pos,uav_vel; 105 105 106 106 uavVrpn->GetPosition(uav_pos); … … 112 112 113 113 AhrsData *CircleFollower::GetReferenceOrientation(void) { 114 Vector2D pos_err, vel_err; // in Uav coordinate system114 Vector2Df pos_err, vel_err; // in Uav coordinate system 115 115 float yaw_ref; 116 116 Euler refAngles; … … 128 128 refAngles.roll=-uY->Output(); 129 129 130 customReferenceOrientation->SetQuaternionAndAngularRates(refAngles.ToQuaternion(),Vector3D (0,0,0));130 customReferenceOrientation->SetQuaternionAndAngularRates(refAngles.ToQuaternion(),Vector3Df(0,0,0)); 131 131 132 132 return customReferenceOrientation; 133 133 } 134 134 135 void CircleFollower::PositionValues(Vector2D &pos_error,Vector2D&vel_error,float &yaw_ref) {136 Vector3D uav_pos,uav_vel; // in VRPN coordinate system137 Vector2D uav_2Dpos,uav_2Dvel; // in VRPN coordinate system135 void CircleFollower::PositionValues(Vector2Df &pos_error,Vector2Df &vel_error,float &yaw_ref) { 136 Vector3Df uav_pos,uav_vel; // in VRPN coordinate system 137 Vector2Df uav_2Dpos,uav_2Dvel; // in VRPN coordinate system 138 138 139 139 uavVrpn->GetPosition(uav_pos); … … 148 148 yaw_ref=yawHold; 149 149 } else { //Circle 150 Vector3D target_pos;151 Vector2D circle_pos,circle_vel;152 Vector2D target_2Dpos;150 Vector3Df target_pos; 151 Vector2Df circle_pos,circle_vel; 152 Vector2Df target_2Dpos; 153 153 154 154 targetVrpn->GetPosition(target_pos); … … 238 238 return; 239 239 } 240 Vector3D uav_pos,target_pos;241 Vector2D uav_2Dpos,target_2Dpos;240 Vector3Df uav_pos,target_pos; 241 Vector2Df uav_2Dpos,target_2Dpos; 242 242 243 243 targetVrpn->GetPosition(target_pos); … … 265 265 yawHold=vrpnQuaternion.ToEuler().yaw; 266 266 267 Vector3D vrpnPosition;267 Vector3Df vrpnPosition; 268 268 uavVrpn->GetPosition(vrpnPosition); 269 269 vrpnPosition.To2Dxy(posHold); -
trunk/demos/CircleFollower/uav/src/CircleFollower.h
r122 r167 54 54 void ExtraCheckJoystick(void); 55 55 const flair::core::AhrsData *GetOrientation(void) const; 56 void AltitudeValues(float &z,float &dz) ;57 void PositionValues(flair::core::Vector2D &pos_error,flair::core::Vector2D&vel_error,float &yaw_ref);56 void AltitudeValues(float &z,float &dz) const; 57 void PositionValues(flair::core::Vector2Df &pos_error,flair::core::Vector2Df &vel_error,float &yaw_ref); 58 58 flair::core::AhrsData *GetReferenceOrientation(void); 59 59 void SignalEvent(Event_t event); … … 61 61 flair::filter::Pid *uX, *uY; 62 62 63 flair::core::Vector2D posHold;63 flair::core::Vector2Df posHold; 64 64 float yawHold; 65 65
Note:
See TracChangeset
for help on using the changeset viewer.