mirror of https://github.com/ArduPilot/ardupilot
These files are not meant to be in master, they should only be in the APM_Camera branch for now
This commit is contained in:
Amilcar Lucas
parent
0fbca87a03
commit
a540f60cc8
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -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
|
||||
Loading…
Reference in New Issue