Payload libraries, code for things like camera control, mount control antenna control should go here.

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

libraries/AP_Payload/AP_Mount.cpp

@@ -0,0 +1,135 @@
+#include "AP_Mount.h"
+
+AP_Mount::AP_Mount(GPS *gps, AP_DCM *dcm)
+{
+	_dcm=dcm;
+	_gps=gps;
+}
+
+void AP_Mount::SetGPSTarget(Location targetGPSLocation)
+{
+	_targetGPSLocation=targetGPSLocation;
+	
+	//set mode
+	_mountmode=gps;
+
+	//update mount position
+	UpDateMount();
+}
+
+void AP_Mount::SetAssisted(int roll, int pitch, int yaw)
+{
+	_targetVector.x = roll;
+	_targetVector.y = pitch;
+	_targetVector.z = yaw;
+
+	//set mode
+	_mountmode=assisted;
+
+	//update mount position
+	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)
+{
+	_mountFPVVector.x=roll;
+	_mountFPVVector.y=pitch;
+	_mountFPVVector.z=yaw;
+
+	//set mode
+	_mountmode=fpv;
+
+	//now update mount position
+	UpDateMount();
+}
+
+//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;
+
+	//set mode
+	_mountmode=landing;
+
+	//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;
+				pitchAngle =  degrees(atan2(-targ.x,targ.z))*100;	//pitch 
+				rollAngle =   degrees(atan2(targ.y,targ.z))*100;	//roll
+				break;
+			}
+		case antenna:
+			{
+				if(_gps->fix) 
+				{
+					CalcGPSTargetVector(&_grndStation);
+				}
+				Matrix3f m = _dcm->get_dcm_transposed();
+				Vector3<float> targ = m*_targetVector;
+				pitchAngle =  degrees(atan2(-targ.x,targ.z))*100;	//pitch 
+				rollAngle =   degrees(atan2(targ.y,targ.z))*100;	//roll
+				break;
+			}
+		default:
+			{
+				//do nothing
+				break;
+			}			
+	}
+}	
+
+void AP_Mount::SetMode(MountMode mode)
+{
+	_mountmode=mode;
+}
+
+void AP_Mount::CalcGPSTargetVector(struct Location *target)
+{
+	_targetVector.x = (target->lng-_gps->longitude) * cos((_gps->latitude+target->lat)/2)*.01113195;
+	_targetVector.y = (target->lat-_gps->latitude)*.01113195;
+	_targetVector.z = (_gps->altitude-target->alt);
+}

libraries/AP_Payload/AP_Mount.h

@@ -0,0 +1,78 @@
+/****************************************************************	          
+    * AP_mount -- library to control a 2 or 3 axis mount.		*   
+    *															*   
+    * Author:  Joe Holdsworth;									*
+	*		   Ritchie Wilson;									*
+	*			Amiclair Lucus;									*   
+    *															*   
+    * Purpose:  Move a 2 or 3 axis mount attached to vehicle,	*
+	*			Used for mount to track targets or stabilise	*
+	*			camera plus	other modes.						*   
+    *															*   
+    * Usage:													*   
+    *			Use in main code to control	mounts attached to	*
+	*			vehicle.										*
+	*															*
+	*Comments:  All angles in degrees * 100, distances in meters*
+	*			unless otherwise stated.						*
+ ***************************************************************/ 
+#ifndef AP_Mount_H
+#define AP_Mount_H
+
+#include <AP_Common.h>
+#include <AP_Math.h>
+#include <AP_GPS.h>
+#include <AP_DCM.h>
+
+//all angles in degrees * 100
+class AP_Mount
+{
+public:
+	//Constructors
+	AP_Mount(GPS *gps, AP_DCM *dcm);
+
+	//enums
+	enum MountMode{
+		gps = 0,
+		stabilise = 1, //note the correct english spelling :)
+		fpv = 2,
+		assisted = 3,
+		antenna = 4,
+		landing = 5,
+		none = 6
+	};
+	
+	//Accessors
+	//used with dcm matrix to calculate target vector
+	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
+	
+	//methods
+	void UpDateMount();
+	void SetMode(MountMode mode);    
+	
+	int pitchAngle; //degrees*100
+	int rollAngle;	//degrees*100
+protected:
+	//methods
+	void CalcGPSTargetVector(struct Location *target);
+	//void CalculateDCM(int roll, int pitch, int yaw);
+	//members
+	AP_DCM			*_dcm;
+	GPS 		*_gps;
+
+	MountMode _mountmode;
+	struct Location _grndStation;
+	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
+
+
+};
+#endif