Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Checkin before beddy byes 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@3154 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
joeholdsworth@gmail.com 2011-08-22 21:50:11 +00:00
parent 659753d7a6
commit 085ca253be
2 changed files with 83 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

109
libraries/AP_Mount/AP_Mount.cpp
View File

@ -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<float> 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<float> 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<float> 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

37
libraries/AP_Mount/AP_Mount.h
View File

@ -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 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