zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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

mission-4.1.18
Amilcar Lucas 14 years ago
parent
13681612c2
commit
a8e68040f5
2 changed files with 0 additions and 283 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 184
      libraries/AP_Mount/AP_Mount.cpp
  2. 99
      libraries/AP_Mount/AP_Mount.h

184
libraries/AP_Mount/AP_Mount.cpp
Unescape View File

@ -1,184 +0,0 @@ @@ -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
Unescape View File

@ -1,99 +0,0 @@ @@ -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
Write Preview
Loading…
Cancel
Save