nightly check in, version not stable.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@3164 f9c3cf11-9bcb-44bc-f272-b75c42450872

libraries/AP_Mount/AP_Mount.cpp

@@ -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 
+				this->CalcMountAnglesFromVector(*targ)
-				rollAngle =   degrees(atan2(targ.y,targ.z))*100;	//roll
 				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 
+				this->CalcMountAnglesFromVector(*targ)
-				rollAngle =   degrees(atan2(targ.y,targ.z))*100;	//roll
 				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;
+			}
+	}
+}

libraries/AP_Mount/AP_Mount.h

@@ -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 SetPitchYaw(int pitchCh, int yawCh);
-	void SetPitchRoll(int pitchCh, int rollCh);
+	//void SetPitchRoll(int pitchCh, int rollCh);
-	void SetPitchRollYaw(int pitchCh, int rollCh, int yawCh);
+	//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;