Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

These files are not meant to be in master, they should only be in the APM_Camera branch for now

Browse Source
This commit is contained in:
Amilcar Lucas 2011-09-09 16:37:42 +02:00
parent 13681612c2
commit a8e68040f5
2 changed files with 0 additions and 283 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

184
libraries/AP_Mount/AP_Mount.cpp
View File

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

99
libraries/AP_Mount/AP_Mount.h
View File

@ -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 <AP_Common.h>
#include <AP_Math.h>
#include <AP_GPS.h>
#include <AP_DCM.h>
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