From a8e68040f5c478f2470d0002c2aadc43b5a0fe7b Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Fri, 9 Sep 2011 16:37:42 +0200 Subject: [PATCH] These files are not meant to be in master, they should only be in the APM_Camera branch for now --- libraries/AP_Mount/AP_Mount.cpp | 184 -------------------------------- libraries/AP_Mount/AP_Mount.h | 99 ----------------- 2 files changed, 283 deletions(-) delete mode 100644 libraries/AP_Mount/AP_Mount.cpp delete mode 100644 libraries/AP_Mount/AP_Mount.h diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp deleted file mode 100644 index f19ba7fe93..0000000000 --- a/libraries/AP_Mount/AP_Mount.cpp +++ /dev/null @@ -1,184 +0,0 @@ -#include "AP_Mount.h" - -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); -{ - -} - -void AP_Mount::SetPitchYaw(Location targetGPSLocation) -{ - _targetGPSLocation=targetGPSLocation; - - //set mode - _mountMode=gps; - - //update mount position - UpDateMount(); -} - -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) -{ - _AssistAngles.x = roll; - _AssistAngles.y = pitch; - _AssistAngles.z = yaw; - - //set mode - _mountMode=assisted; - - //update mount position - UpDateMount(); -} - -//sets the servo angles for FPV, note angles are * 100 -void AP_Mount::SetMountFreeRoam(int roll, int pitch, int yaw) -{ - _RoamAngles.x=roll; - _RoamAngles.y=pitch; - _RoamAngles.z=yaw; - - //set mode - _mountMode=roam; - - //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) -{ - _LandingAngles.x=roll; - _LandingAngles.y=pitch; - _LandingAngles.z=yaw; - - //set mode - _mountMode=landing; - - //now update mount position - UpDateMount(); -} - -void AP_Mount::SetNone() -{ - //set mode - _mountMode=none; - - //now update mount position - UpDateMount(); -} - -void AP_Mount::UpDateMount() -{ - switch(_mountMode) - { - case gps: - { - if(_gps->fix) - { - CalcGPSTargetVector(&_targetGPSLocation); - } - m = _dcm->get_dcm_transposed(); - targ = m*_GPSVector; - this->CalcMountAnglesFromVector(*targ) - break; - } - case stabilise: - { - //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(); - targ = m*_AssistVector; - this->CalcMountAnglesFromVector(*targ) - break; - } - case landing: - { - pitchAngle=100*_RoamAngles.y; - rollAngle=100*_RoamAngles.x; - yawAngle=100*_RoamAngles.z; - break; - } - case none: - { - //do nothing - 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); -} - -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; - } - } -} diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h deleted file mode 100644 index 4ee2930edc..0000000000 --- a/libraries/AP_Mount/AP_Mount.h +++ /dev/null @@ -1,99 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -/************************************************************ -* 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. * -* 1. initialise class * -* 2. setMounttype * * -* * -*Comments: All angles in degrees * 100, distances in meters* -* unless otherwise stated. * - ************************************************************/ -#ifndef AP_Mount_H -#define AP_Mount_H - -#include -#include -#include -#include - -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_Var::Key key, const prog_char_t *name); - - //enums - enum MountMode{ - gps = 0, - stabilise = 1, //note the correct english spelling :) - roam = 2, - assisted = 3, - landing = 4, - none = 5 - }; - - enum MountType{ - pitch_yaw = 0, - 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 SetGPSTarget(Location targetGPSLocation); //used to tell the mount to track GPS location - void SetAssisted(int roll, int pitch, int yaw); - 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(); - void SetMode(MountMode mode); - - int pitchAngle; //degrees*100 - int rollAngle; //degrees*100 - int yawAngle; //degrees*100 - -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; - MountType _mountType; - - struct Location _targetGPSLocation; - 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