aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

nightly check in, version not stable.

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@3164 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
joeholdsworth@gmail.com 2011-08-23 20:36:30 +00:00
parent cce5ad68bd
commit 9e36c13f9c
2 changed files with 67 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
libraries/AP_Mount/AP_Mount.cpp
View File

@ -1,9 +1,13 @@
#include "AP_Mount.h"
AP_Mount::AP_Mount(GPS *gps, AP_DCM *dcm)
AP_Mount::AP_Mount(GPS *gps, AP_DCM *dcm, AP_Var::Key key, const prog_char_t *name):
_group(key, name),
_mountMode(&_group, 0, 0, name ? PSTR("MODE") : 0), // suppress name if group has no name
_mountType(&_group, 0, 0, name ? PSTR("TYPE") : 0),
_dcm(dcm);
_gps(gps);
{
_dcm=dcm;
_gps=gps;
}
void AP_Mount::SetPitchYaw(Location targetGPSLocation)
@ -11,7 +15,7 @@ void AP_Mount::SetPitchYaw(Location targetGPSLocation)
_targetGPSLocation=targetGPSLocation;
//set mode
_mountmode=gps;
_mountMode=gps;
//update mount position
UpDateMount();
@ -22,7 +26,7 @@ void AP_Mount::SetGPSTarget(Location targetGPSLocation)
_targetGPSLocation=targetGPSLocation;
//set mode
_mountmode=gps;
_mountMode=gps;
//update mount position
UpDateMount();
@ -35,7 +39,7 @@ void AP_Mount::SetAssisted(int roll, int pitch, int yaw)
_AssistAngles.z = yaw;
//set mode
_mountmode=assisted;
_mountMode=assisted;
//update mount position
UpDateMount();
@ -49,7 +53,7 @@ void AP_Mount::SetMountFreeRoam(int roll, int pitch, int yaw)
_RoamAngles.z=yaw;
//set mode
_mountmode=roam;
_mountMode=roam;
//now update mount position
UpDateMount();
@ -63,7 +67,7 @@ void AP_Mount::SetMountLanding(int roll, int pitch, int yaw)
_LandingAngles.z=yaw;
//set mode
_mountmode=landing;
_mountMode=landing;
//now update mount position
UpDateMount();
@ -72,7 +76,7 @@ void AP_Mount::SetMountLanding(int roll, int pitch, int yaw)
void AP_Mount::SetNone()
{
//set mode
_mountmode=none;
_mountMode=none;
//now update mount position
UpDateMount();
@ -80,7 +84,7 @@ void AP_Mount::SetNone()
void AP_Mount::UpDateMount()
{
switch(_mountmode)
switch(_mountMode)
{
case gps:
{
@ -90,8 +94,7 @@ void AP_Mount::UpDateMount()
}
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
this->CalcMountAnglesFromVector(*targ)
break;
}
case stabilise:
@ -109,10 +112,8 @@ void AP_Mount::UpDateMount()
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
this->CalcMountAnglesFromVector(*targ)
break;
}
case landing:
@ -137,7 +138,7 @@ void AP_Mount::UpDateMount()
void AP_Mount::SetMode(MountMode mode)
{
_mountmode=mode;
_mountMode=mode;
}
void AP_Mount::CalcGPSTargetVector(struct Location *target)
@ -146,3 +147,38 @@ void AP_Mount::CalcGPSTargetVector(struct Location *target)
_targetVector.y = (target->lat-_gps->latitude)*.01113195;
_targetVector.z = (_gps->altitude-target->alt);
}
void AP_Mount::CalcMountAnglesFromVector(Vector3f *targ)
{
switch(_mountType)
{
case pitch_yaw:
{
//need to tidy up maths for below
pitchAngle = atan2((sqrt(sq(targ.y) + sq(targ.x)) * .01113195), targ.z) * -1;
yawAngle = 9000 + atan2(-targ.y, targ.x) * 5729.57795;
break;
}
case pitch_roll:
{
pitchAngle = degrees(atan2(-targ.x,targ.z))*100; //pitch
rollAngle = degrees(atan2(targ.y,targ.z))*100; //roll
break;
}
case pitch_roll_yaw:
{
//to do
break;
}
case none:
{
//do nothing
break;
}
default:
{
//do nothing
break;
}
}
}

22
libraries/AP_Mount/AP_Mount.h
View File

@ -13,6 +13,8 @@
* *
* Usage: Use in main code to control mounts attached to *
* vehicle. *
* 1. initialise class *
* 2. setMounttype * *
* *
*Comments: All angles in degrees * 100, distances in meters*
* unless otherwise stated. *
@ -27,9 +29,12 @@
class AP_Mount
{
protected:
AP_Var_group _group; // must be before all vars to keep ctor init order correct
public:
//Constructors
AP_Mount(GPS *gps, AP_DCM *dcm);
AP_Mount(GPS *gps, AP_DCM *dcm, AP_Var::Key key, const prog_char_t *name);
//enums
enum MountMode{
@ -43,14 +48,15 @@ public:
enum MountType{
pitch_yaw = 0,
pitch_roll = 1, //note the correct english spelling :)
pitch_roll = 1,
pitch_roll_yaw = 2,
none = 3;
};
//Accessors
void SetPitchYaw(int pitchCh, int yawCh);
void SetPitchRoll(int pitchCh, int rollCh);
void SetPitchRollYaw(int pitchCh, int rollCh, int yawCh);
//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);
@ -65,15 +71,17 @@ public:
int pitchAngle; //degrees*100
int rollAngle; //degrees*100
int yawAngle; //degrees*100
protected:
private:
//methods
void CalcGPSTargetVector(struct Location *target);
void CalcMountAnglesFromVector(Vector3f *targ);
//void CalculateDCM(int roll, int pitch, int yaw);
//members
AP_DCM *_dcm;
GPS *_gps;
MountMode _mountmode;
MountMode _mountMode;
MountType _mountType;
struct Location _targetGPSLocation;