From c140113583a7311588a2cf40a428d834afe14cd5 Mon Sep 17 00:00:00 2001 From: "joeholdsworth@gmail.com" Date: Mon, 22 Aug 2011 21:50:11 +0000 Subject: [PATCH] Checkin before beddy byes git-svn-id: https://arducopter.googlecode.com/svn/trunk@3154 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/AP_Mount/AP_Mount.cpp | 109 ++++++++++++++++++-------------- libraries/AP_Mount/AP_Mount.h | 39 +++++++----- 2 files changed, 83 insertions(+), 65 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 4cf9f636b0..6e387e2c6d 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -6,6 +6,17 @@ AP_Mount::AP_Mount(GPS *gps, AP_DCM *dcm) _gps=gps; } +void AP_Mount::SetPitchYaw(Location targetGPSLocation) +{ + _targetGPSLocation=targetGPSLocation; + + //set mode + _mountmode=gps; + + //update mount position + UpDateMount(); +} + void AP_Mount::SetGPSTarget(Location targetGPSLocation) { _targetGPSLocation=targetGPSLocation; @@ -19,9 +30,9 @@ void AP_Mount::SetGPSTarget(Location targetGPSLocation) void AP_Mount::SetAssisted(int roll, int pitch, int yaw) { - _targetVector.x = roll; - _targetVector.y = pitch; - _targetVector.z = yaw; + _AssistAngles.x = roll; + _AssistAngles.y = pitch; + _AssistAngles.z = yaw; //set mode _mountmode=assisted; @@ -30,27 +41,15 @@ void AP_Mount::SetAssisted(int roll, int pitch, int yaw) UpDateMount(); } -void AP_Mount::SetAntenna(Location grndStation) -{ - _grndStation=grndStation; - - //set mode - _mountmode=antenna; - - //update mount position - UpDateMount(); -} - - //sets the servo angles for FPV, note angles are * 100 -void AP_Mount::SetMountFPV(int roll, int pitch, int yaw) +void AP_Mount::SetMountFreeRoam(int roll, int pitch, int yaw) { - _mountFPVVector.x=roll; - _mountFPVVector.y=pitch; - _mountFPVVector.z=yaw; + _RoamAngles.x=roll; + _RoamAngles.y=pitch; + _RoamAngles.z=yaw; //set mode - _mountmode=fpv; + _mountmode=roam; //now update mount position UpDateMount(); @@ -59,9 +58,9 @@ void AP_Mount::SetMountFPV(int roll, int pitch, int yaw) //sets the servo angles for landing, note angles are * 100 void AP_Mount::SetMountLanding(int roll, int pitch, int yaw) { - _mountLanding.x=roll; - _mountLanding.y=pitch; - _mountLanding.z=yaw; + _LandingAngles.x=roll; + _LandingAngles.y=pitch; + _LandingAngles.z=yaw; //set mode _mountmode=landing; @@ -70,50 +69,64 @@ void AP_Mount::SetMountLanding(int roll, int pitch, int yaw) UpDateMount(); } +void AP_Mount::SetNone() +{ + //set mode + _mountmode=none; + + //now update mount position + UpDateMount(); +} + void AP_Mount::UpDateMount() { switch(_mountmode) { - case fpv: - { - pitchAngle=100*_mountFPVVector.y; - rollAngle=100*_mountFPVVector.x; - break; - } - case assisted: - { - Matrix3f m = _dcm->get_dcm_transposed(); - //rotate vector - //to do: find out notion of x y convention - Vector3 targ = m*_targetVector; - pitchAngle = degrees(atan2(-targ.x,targ.z))*100; //pitch - rollAngle = degrees(atan2(targ.y,targ.z))*100; //roll - break; - } case gps: { if(_gps->fix) { CalcGPSTargetVector(&_targetGPSLocation); } - Matrix3f m = _dcm->get_dcm_transposed(); - Vector3 targ = m*_targetVector; + m = _dcm->get_dcm_transposed(); + targ = m*_GPSVector; pitchAngle = degrees(atan2(-targ.x,targ.z))*100; //pitch rollAngle = degrees(atan2(targ.y,targ.z))*100; //roll break; } - case antenna: + case stabilise: { - if(_gps->fix) - { - CalcGPSTargetVector(&_grndStation); - } - Matrix3f m = _dcm->get_dcm_transposed(); - Vector3 targ = m*_targetVector; + //to do + break; + } + case roam: + { + pitchAngle=100*_RoamAngles.y; + rollAngle=100*_RoamAngles.x; + yawAngle=100*_RoamAngles.z; + break; + } + case assisted: + { + m = _dcm->get_dcm_transposed(); + //rotate vector + targ = m*_AssistVector; pitchAngle = degrees(atan2(-targ.x,targ.z))*100; //pitch rollAngle = degrees(atan2(targ.y,targ.z))*100; //roll break; } + case landing: + { + pitchAngle=100*_RoamAngles.y; + rollAngle=100*_RoamAngles.x; + yawAngle=100*_RoamAngles.z; + break; + } + case none: + { + //do nothing + break; + } default: { //do nothing diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index dbcd5f9766..d6c822b2a8 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -35,11 +35,10 @@ public: enum MountMode{ gps = 0, stabilise = 1, //note the correct english spelling :) - fpv = 2, + roam = 2, assisted = 3, - antenna = 4, - landing = 5, - none = 6 + landing = 4, + none = 5 }; enum MountType{ @@ -49,16 +48,15 @@ public: }; //Accessors - void SetPitchYaw(); - void SetPitchRoll(); - void SetPitchRollYaw(); + void SetPitchYaw(int pitchCh, int yawCh); + void SetPitchRoll(int pitchCh, int rollCh); + void SetPitchRollYaw(int pitchCh, int rollCh, int yawCh); + void SetGPSTarget(Location targetGPSLocation); //used to tell the mount to track GPS location void SetAssisted(int roll, int pitch, int yaw); - void SetAntenna(Location grndStation); - - //action - void SetMountFPV(int roll, int pitch, int yaw); //used in the FPV, - void SetMountLanding(int roll, int pitch, int yaw); //set mount landing position + void SetMountFreeRoam(int roll, int pitch, int yaw);//used in the FPV for example, + void SetMountLanding(int roll, int pitch, int yaw); //set mount landing position + void SetNone(); //methods void UpDateMount(); @@ -66,21 +64,28 @@ public: int pitchAngle; //degrees*100 int rollAngle; //degrees*100 + int yawAngle; //degrees*100 protected: //methods void CalcGPSTargetVector(struct Location *target); //void CalculateDCM(int roll, int pitch, int yaw); //members - AP_DCM *_dcm; + AP_DCM *_dcm; GPS *_gps; MountMode _mountmode; - struct Location _grndStation; + MountType _mountType; + struct Location _targetGPSLocation; - Vector3f _targetVector; //target vector calculated stored in meters - Vector3i _mountFPVVector; //used for FPV mode vector.x = roll vector.y = pitch, vector.z=yaw - Vector3i _mountLanding; //landing position for mount, vector.x = roll vector.y = pitch, vector.z=yaw + Vector3f _GPSVector; //target vector calculated stored in meters + + Vector3i _RoamAngles; //used for roam mode vector.x = roll vector.y = pitch, vector.z=yaw + Vector3i _LandingAngles; //landing position for mount, vector.x = roll vector.y = pitch, vector.z=yaw + Vector3i _AssistAngles; //used to keep angles that user has supplied from assisted targetting + Vector3f _AssistVector; //used to keep vector calculated from _AssistAngles + Matrix3f _m; //holds 3 x 3 matrix, var is used as temp in calcs + Vector3f _targ; //holds target vector, var is used as temp in calcs }; #endif \ No newline at end of file